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>
  19
  20 #define PERIOD  1           /* period of motor control updates */
  21 #define TEST 0
  22
  23 char motorWASD = ' '; /* current motor drive command */
  24 float targetAngle = 0.0;
  25 bool targetAngleSet = false;
  26
  27 static ZumoMotors motors; /* Zumo motor driver provided by Pololu */
  28
  29 PIDController steerPID(0.01f, 0.0f, 0.0f);
  30 PIDController spinPID(2.0f, 0.0f, 0.0f);
  31
  32 Utilities::MotorInfo motorStatus;
  33 int count = 0;
  34 bool timedOut = false;
  35 Utilities util;
  36 long initTime;
  37
  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);
  42
  43 /*
  44  *  ======== motorSetup ========
  45  */
  46 void motorSetup(void)
  47 {
  48     Serial.begin(9600);
  49     Serial.println("motorSetup ...");
  50
  51     /* initialize the Pololu driver motor library */
  52     ZumoMotors::setRightSpeed(0);
  53     ZumoMotors::setLeftSpeed(0);
  54     motorStatus.leftSpeed = 0;
  55     motorStatus.rightSpeed = 0;
  56
  57     Serial.println("motorSetup done.");
  58     util = Utilities();
  59 }
  60
  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     }
  78
  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             }
  96
  97             case 'd':
  98             case 'a': {
  99                 targetAngleSet = false;
 100                 drive(motorWASD, 100, PERIOD);
 101                 break;
 102             }
 103
 104             case 't': {
 105                 turnAngle(angleToTurn, driveRate);
 106                 break;
 107             }
 108             case 'g': {
 109                 driveStraight(driveTime, driveRate);
 110                 break;
 111             }
 112
 113             default: {
 114                 targetAngleSet = false;
 115                 motorWASD = ' ';
 116                 drive(' ', 0, 10);
 117                 break;
 118             }
 119         }
 120     }
 121 }
 122
 123 /*
 124  *  ======== clip ========
 125  */
 126 static int clip(int speed)
 127 {
 128     if (speed < -400) {
 129         speed = -400;
 130     }
 131     else if (speed > 400) {
 132         speed = 400;
 133     }
 134     return (speed);
 135 }
 136
 137 static float clip(float speed)
 138 {
 139     if (speed < -400) {
 140         speed = -400.0;
 141     }
 142     else if (speed > 400) {
 143         speed = 400;
 144     }
 145     return (speed);
 146 }
 147
 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)
 155 {
 156     static int leftSpeed = 0;
 157     static int rightSpeed = 0;
 158
 159     while (duration > 0) {
 160         duration--;
 161
 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             }
 174
 175             case 's': { /* backward */
 176                 leftSpeed = next(leftSpeed, -goal);
 177                 rightSpeed = next(rightSpeed, -goal);
 178                 break;
 179             }
 180
 181             case 'd': { /* turn right */
 182                 leftSpeed = next(leftSpeed, goal);
 183                 rightSpeed = next(rightSpeed, -goal);
 184                 break;
 185             }
 186
 187             case 'a': { /* turn left */
 188                 leftSpeed = next(leftSpeed, -goal);;
 189                 rightSpeed = next(rightSpeed, goal);
 190                 break;
 191             }
 192
 193             default: {
 194                 break;
 195             }
 196         }
 197
 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;
 207
 208         /* sleep for 1 ms (so duration is in milliseconds) */
 209         delay(1);
 210     }
 211 }
 212
 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)
 218 {
 219     int tmp = (goal - cur) * 0.0625f + cur;
 220     return (tmp == cur ? goal : tmp);
 221 }
 222
 223 /*
 224  *  ======== turnAngle ========
 225  *  Turn an angle using a PID controller
 226  */
 227 void turnAngle(float angle, float rate)
 228 {
 229     float error;
 230     float power;
 231
 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         }
 246
 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         }
 254
 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         }
 264
 265         motorStatus.leftSpeed = power;
 266         motorStatus.rightSpeed = -power;
 267
 268         ZumoMotors::setLeftSpeed(power);
 269         ZumoMotors::setRightSpeed(-power);
 270
 271         motorStatus.error = error;
 272         motorStatus.time = (float)millis();
 273     }
 274
 275     delay(10);
 276 }
 277
 278 /*
 279  *  ======== driveStraight ========
 280  *  Use the gyro and PID control to drive the robot straight
 281  */
 282 void driveStraight(float numSeconds, float rate)
 283 {
 284     bool forward = true;
 285
 286     if (rate < 0) {
 287         forward = false;
 288         rate = -rate;
 289     }
 290
 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);
 304
 305         if (!forward) {
 306             float temp = lTotal;
 307             lTotal = -rTotal;
 308             rTotal = -temp;
 309         }
 310
 311         if (timedOut) {
 312             lTotal = 0;
 313             rTotal = 0;
 314             motorWASD = ' ';
 315         }
 316         else {
 317             timedOut = ((millis() - initTime) > ((int)(numSeconds * 1000.0f)));
 318         }
 319
 320         ZumoMotors::setLeftSpeed(lTotal);
 321         ZumoMotors::setRightSpeed(rTotal);
 322
 323         motorStatus.error = error;
 324         motorStatus.leftSpeed = lTotal;
 325         motorStatus.rightSpeed = rTotal;
 326         motorStatus.time = (float)millis();
 327     }
 328
 329     delay(10);
 330 }