remove unused error averaging from Utilities
[zumo-cc3200/zumo-cc3200.git] / src / Energia / libraries / ZumoCC3200 / examples / AssistedDrive / MotorControlLoop.ino
1 /*
2  *  ======== motorLoop ========
3  *  This sketch controls the motors on the Zumo by polling a global
4  *  variable, motorWASD, once per PERIOD milliseconds for one of the
5  *  following motor commands:
6  *      'w' - drive forward
7  *      's' - drive backward
8  *      'a' - turn left
9  *      'd' - turn right
10  *      ' ' - stop
11  *
12  *  Other sketches or interrupts can control the zumo by simply writing the
13  *  desired command to motorWASD.
14  */
15 #include <Energia.h>
16 #include <math.h>
17 #include <PIDController.h>
18 #include <ZumoMotors.h>
20 #define PERIOD  1           /* period of motor control updates */
21 #define TEST 0
23 char motorWASD = ' '; /* current motor drive command */
24 float targetAngle = 0.0;
25 bool targetAngleSet = false;
27 static ZumoMotors motors; /* Zumo motor driver provided by Pololu */
29 PIDController steerPID(0.01f, 0.0f, 0.0f);
30 PIDController spinPID(2.0f, 0.0f, 0.0f);
32 Utilities::MotorInfo motorStatus;
33 int count = 0;
34 bool timedOut = false;
35 Utilities util;
36 long initTime;
38 static int   clip(int speed);
39 static float clip(float speed);
40 static void  drive(char wasd, int speed, unsigned int duration);
41 static int   next(int cur, int goal);
43 /*
44  *  ======== motorSetup ========
45  */
46 void motorSetup(void)
47 {
48     Serial.begin(9600);
49     Serial.println("motorSetup ...");
51     /* initialize the Pololu driver motor library */
52     ZumoMotors::setRightSpeed(0);
53     ZumoMotors::setLeftSpeed(0);
54     motorStatus.leftSpeed = 0;
55     motorStatus.rightSpeed = 0;
57     Serial.println("motorSetup done.");
58     util = Utilities();
59 }
61 /*
62  *  ======== motorLoop ========
63  *
64  *  Based on user input, drive open loop or
65  *  perform closed-loop maneuvers
66  */
67 void motorLoop(void)
68 {
69     static char lastWASD = ' ';
70     if (lastWASD != motorWASD) {
71         lastWASD = motorWASD;
72         
73         Serial.print("new motor cmd: "); Serial.println(motorWASD);
74         Serial.print("  is angle set: "); Serial.println(targetAngleSet);
75         Serial.print("  target angle: "); Serial.println(targetAngle);
76         Serial.print("  timed out   : "); Serial.println(timedOut);
77     }
79     if (isZeroing) {
80         spinPID = PIDController(2.0f, 0.0f, 0.0f);
81         steerPID = PIDController(0.01f, 0.0f, 0.0f);
82         ZumoMotors::setLeftSpeed(0);
83         ZumoMotors::setRightSpeed(0);
84         motorStatus.leftSpeed = 0;
85         motorStatus.rightSpeed = 0;
86         delay(500);
87     }
88     else {
89         switch (motorWASD) {
90             case 's':
91             case 'w': {
92                 targetAngleSet = false;
93                 drive(motorWASD, 250, PERIOD);
94                 break;
95             }
97             case 'd':
98             case 'a': {
99                 targetAngleSet = false;
100                 drive(motorWASD, 100, PERIOD);
101                 break;
102             }
104             case 't': {
105                 turnAngle(angleToTurn, driveRate);
106                 break;
107             }
108             case 'g': {
109                 driveStraight(driveTime, driveRate);
110                 break;
111             }
113             default: {
114                 targetAngleSet = false;
115                 motorWASD = ' ';
116                 drive(' ', 0, 10);
117                 break;
118             }
119         }
120     }
123 /*
124  *  ======== clip ========
125  */
126 static int clip(int speed)
128     if (speed < -400) {
129         speed = -400;
130     }
131     else if (speed > 400) {
132         speed = 400;
133     }
134     return (speed);
137 static float clip(float speed)
139     if (speed < -400) {
140         speed = -400.0;
141     }
142     else if (speed > 400) {
143         speed = 400;
144     }
145     return (speed);
148 /*
149  *  ======== drive ========
150  *  Drive motors in the direction and speed specified by wasd and goal
151  *
152  *  Note: negative goal values imply a reversal of the wasd direction
153  */
154 static void drive(char wasd, int goal, unsigned int duration)
156     static int leftSpeed = 0;
157     static int rightSpeed = 0;
158     
159     while (duration > 0) {
160         duration--;
162         /* gradually adjust curent speeds to goal */
163         switch (wasd) {
164             case ' ': { /* stop */
165                 leftSpeed = next(leftSpeed, 0);
166                 rightSpeed = next(rightSpeed, 0);
167                 break;
168             }
169             case 'w': { /* forward */
170                 leftSpeed = next(leftSpeed, goal);
171                 rightSpeed = next(rightSpeed, goal);
172                 break;
173             }
175             case 's': { /* backward */
176                 leftSpeed = next(leftSpeed, -goal);
177                 rightSpeed = next(rightSpeed, -goal);
178                 break;
179             }
181             case 'd': { /* turn right */
182                 leftSpeed = next(leftSpeed, goal);
183                 rightSpeed = next(rightSpeed, -goal);
184                 break;
185             }
187             case 'a': { /* turn left */
188                 leftSpeed = next(leftSpeed, -goal);;
189                 rightSpeed = next(rightSpeed, goal);
190                 break;
191             }
193             default: {
194                 break;
195             }
196         }
198         /* clip speeds to allowable range */
199         leftSpeed = clip(leftSpeed);
200         rightSpeed = clip(rightSpeed);
201     
202         /* set motor speeds */
203         ZumoMotors::setLeftSpeed(leftSpeed);
204         ZumoMotors::setRightSpeed(rightSpeed);
205         motorStatus.leftSpeed = leftSpeed;
206         motorStatus.rightSpeed = rightSpeed;
208         /* sleep for 1 ms (so duration is in milliseconds) */
209         delay(1);
210     }
213 /*
214  *  ======== next ========
215  *  Compute the next motor speed value given the cur speed and a new goal
216  */
217 static int next(int cur, int goal)
219     int tmp = (goal - cur) * 0.0625f + cur;
220     return (tmp == cur ? goal : tmp);
223 /*
224  *  ======== turnAngle ========
225  *  Turn an angle using a PID controller
226  */
227 void turnAngle(float angle, float rate)
229     float error;
230     float power;
232     if (!targetAngleSet) {
233         float currHeading = IMUManager::getGyroYaw();
234         targetAngle = Utilities::wrapAngle(currHeading + angle);
235         targetAngleSet = true;
236     }
237     else {
238         error = Utilities::wrapAngle(IMUManager::getGyroYaw() - targetAngle);
239         power = spinPID.calculate(error);
240         bool done = Utilities::inRange(error, -0.5f, 0.5f);
241         if (done) {
242             motorWASD = ' ';
243             targetAngleSet = false;
244             power = 0;
245         }
247         /* if power is too low, the bot doesn't turn */
248         if (power > 0 && power < 70) {
249             power = 70;
250         }
251         else if (power < 0 && power > -70) {
252             power = -70;
253         }
255         /* if power is higher than 250, a buildup of error in the
256          * integrated gyro angle occurs
257          */
258         if (power > 250) {
259             power = 250;
260         }
261         else if (power < -250) {
262             power = -250;
263         }
265         motorStatus.leftSpeed = power;
266         motorStatus.rightSpeed = -power;
268         ZumoMotors::setLeftSpeed(power);
269         ZumoMotors::setRightSpeed(-power);
271         motorStatus.error = error;
272         motorStatus.time = (float)millis();
273     }
275     delay(10);
278 /*
279  *  ======== driveStraight ========
280  *  Use the gyro and PID control to drive the robot straight
281  */
282 void driveStraight(float numSeconds, float rate)
284     bool forward = true;
285     
286     if (rate < 0) {
287         forward = false;
288         rate = -rate;
289     }
291     if (!targetAngleSet) {
292         float currHeading = IMUManager::getGyroYaw();
293         targetAngle = currHeading;
294         timedOut = false;
295         targetAngleSet = true;
296         initTime = millis();
297     }
298     else {
299         float error = util.wrapAngle(IMUManager::getGyroYaw() - targetAngle);
300         float power = steerPID.calculate(error);
301         power = util.saturate(power, rate - 1.0f, 1.0f - rate);
302         float lTotal = clip((rate + power) * 400);
303         float rTotal = clip((rate - power) * 400);
305         if (!forward) {
306             float temp = lTotal;
307             lTotal = -rTotal;
308             rTotal = -temp;
309         }
311         if (timedOut) {
312             lTotal = 0;
313             rTotal = 0;
314             motorWASD = ' ';
315         }
316         else {
317             timedOut = ((millis() - initTime) > ((int)(numSeconds * 1000.0f)));
318         }
320         ZumoMotors::setLeftSpeed(lTotal);
321         ZumoMotors::setRightSpeed(rTotal);
323         motorStatus.error = error;
324         motorStatus.leftSpeed = lTotal;
325         motorStatus.rightSpeed = rTotal;
326         motorStatus.time = (float)millis();
327     }
329     delay(10);