add ability to build and upload sketches from the bash command line (via beta ino2cpp...
[zumo-cc3200/zumo-cc3200.git] / src / Energia / ZumoTest / ZumoMotors.cpp
1 #include "ZumoMotors.h"
3 #define REDBEAR 1
5 #ifdef REDBEAR
6 // redbear cc3200 (PWM only available on pins 5 and 6)
7 #define PWM_L 6
8 #define PWM_R 5 
9 #else
10 #define PWM_L 10
11 #define PWM_R 9
12 #endif
14 #define DIR_L 8
15 #define DIR_R 7
17 #if defined(__AVR_ATmega168__) || defined(__AVR_ATmega328P__) || defined (__AVR_ATmega32U4__)
18   #define USE_20KHZ_PWM
19 #endif
20 #undef USE_20KHZ_PWM // redbear cc3200 (need portable, non-AVR, code)
22 static boolean flipLeft = false;
23 static boolean flipRight = false;
25 // constructor (doesn't do anything)
26 ZumoMotors::ZumoMotors()
27 {
28 }
30 // initialize timer1 to generate the proper PWM outputs to the motor drivers
31 void ZumoMotors::init2()
32 {
33   pinMode(PWM_L,  OUTPUT);
34   pinMode(PWM_R,  OUTPUT);
35   pinMode(DIR_L, OUTPUT);
36   pinMode(DIR_R, OUTPUT);
37 #ifdef REDBEAR
38   /* redbear pins 5 & 6 are connected to pins 9 & 10, so we these to be input
39    * so they don't inadvertently drive
40    */
41   pinMode(10, INPUT);    
42   pinMode(9, INPUT);
43 #endif
45 #ifdef USE_20KHZ_PWM
46   // Timer 1 configuration
47   // prescaler: clockI/O / 1
48   // outputs enabled
49   // phase-correct PWM
50   // top of 400
51   //
52   // PWM frequency calculation
53   // 16MHz / 1 (prescaler) / 2 (phase-correct) / 400 (top) = 20kHz
54   TCCR1A = 0b10100000;
55   TCCR1B = 0b00010001;
56   ICR1 = 400;
57 #endif
58 }
60 // enable/disable flipping of left motor
61 void ZumoMotors::flipLeftMotor(boolean flip)
62 {
63   flipLeft = flip;
64 }
66 // enable/disable flipping of right motor
67 void ZumoMotors::flipRightMotor(boolean flip)
68 {
69   flipRight = flip;
70 }
72 // set speed for left motor; speed is a number between -400 and 400
73 void ZumoMotors::setLeftSpeed(int speed)
74 {
75   init(); // initialize if necessary
76     
77   boolean reverse = 0;
78   
79   if (speed < 0)
80   {
81     speed = -speed; // make speed a positive quantity
82     reverse = 1;    // preserve the direction
83   }
84   if (speed > 400)  // Max 
85     speed = 400;
86     
87 #ifdef USE_20KHZ_PWM
88   OCR1B = speed;
89 #else
90   analogWrite(PWM_L, speed * 51 / 80); // default to using analogWrite, mapping 400 to 255
91 #endif 
93   if (reverse ^ flipLeft) // flip if speed was negative or flipLeft setting is active, but not both
94     digitalWrite(DIR_L, HIGH);
95   else
96     digitalWrite(DIR_L, LOW);
97 }
99 // set speed for right motor; speed is a number between -400 and 400
100 void ZumoMotors::setRightSpeed(int speed)
102   init(); // initialize if necessary
103     
104   boolean reverse = 0;
105   
106   if (speed < 0)
107   {
108     speed = -speed;  // Make speed a positive quantity
109     reverse = 1;  // Preserve the direction
110   }
111   if (speed > 400)  // Max PWM dutycycle
112     speed = 400;
113     
114 #ifdef USE_20KHZ_PWM
115   OCR1A = speed;
116 #else
117   analogWrite(PWM_R, speed * 51 / 80); // default to using analogWrite, mapping 400 to 255
118 #endif
120   if (reverse ^ flipRight) // flip if speed was negative or flipRight setting is active, but not both
121     digitalWrite(DIR_R, HIGH);
122   else
123     digitalWrite(DIR_R, LOW);
126 // set speed for both motors
127 void ZumoMotors::setSpeeds(int leftSpeed, int rightSpeed)
129   setLeftSpeed(leftSpeed);
130   setRightSpeed(rightSpeed);