[zumo-cc3200/zumo-cc3200.git] / src / Energia / libraries / ZumoCC3200 / examples / UDPBroadcast / motorLoop.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 <PIDController.h>
17 #include <ZumoMotors.h>
19 #define PERIOD 1 /* period of motor control updates */
21 char motorWASD = ' '; /* current motor drive command */
23 static ZumoMotors motors; /* Zumo motor driver provided by Pololu */
25 PIDController steerPID;
27 static int clip(int speed);
28 static void drive(char wasd, int goal, unsigned int duration);
30 /*
31 * ======== motorSetup ========
32 */
33 void motorSetup(void)
34 {
35 Serial.println("motorSetup ...");
36 /* initialize the Pololu driver motor library */
37 motors.setRightSpeed(0);
38 motors.setLeftSpeed(0);
40 /* setup an LED to indcate forward/backward movement or turning */
41 pinMode(MAIN_LED_PIN, OUTPUT);
42 Serial.println("motorSetup done.");
44 steerPID = PIDController(0.01717, 0.0, 0.003);
45 }
47 /*
48 * ======== motorLoop ========
49 */
50 void motorLoop(void)
51 {
52 /* state used to blink LED */
53 static unsigned count = 0;
54 static char state = 0;
56 switch (motorWASD) {
57 case 's':
58 case 'w': {
59 /* illuminate LED while driving */
60 digitalWrite(MAIN_LED_PIN, HIGH);
61 drive(motorWASD, 200, PERIOD);
62 break;
63 }
65 case 'd':
66 case 'a': {
67 /* blink LED while turning */
68 if (count == ((count / 100) * 100)) {
69 state ^= 1;
70 digitalWrite(MAIN_LED_PIN, state);
71 }
72 drive(motorWASD, 100, PERIOD);
73 break;
74 }
76 default: {
77 /* turn off LED while stopped */
78 motorWASD = ' ';
79 digitalWrite(MAIN_LED_PIN, LOW);
80 drive(' ', 0, 10);
81 break;
82 }
83 }
85 count++;
86 }
88 /*
89 * ======== clip ========
90 */
91 static int clip(int speed)
92 {
93 if (speed < -400) {
94 speed = -400;
95 }
96 else if (speed > 400) {
97 speed = 400;
98 }
99 return (speed);
100 }
102 /*
103 * ======== drive ========
104 * Drive motors in the direction and speed specified by wasd and goal
105 *
106 * Note: negative goal values imply a reversal of the wasd direction
107 */
108 static void drive(char wasd, int goal, unsigned int duration)
109 {
110 static int leftSpeed = 0;
111 static int rightSpeed = 0;
113 while (duration > 0) {
114 duration--;
116 /* gradually adjust curent speeds to goal */
117 switch (wasd) {
118 case ' ': { /* stop */
119 leftSpeed = next(leftSpeed, 0);
120 rightSpeed = next(rightSpeed, 0);
121 break;
122 }
123 case 'w': { /* forward */
124 leftSpeed = next(leftSpeed, goal);
125 rightSpeed = next(rightSpeed, goal);
126 break;
127 }
129 case 's': { /* backward */
130 leftSpeed = next(leftSpeed, -goal);
131 rightSpeed = next(rightSpeed, -goal);
132 break;
133 }
135 case 'd': { /* turn right */
136 leftSpeed = next(leftSpeed, goal);
137 rightSpeed = next(rightSpeed, -goal);
138 break;
139 }
141 case 'a': { /* turn left */
142 leftSpeed = next(leftSpeed, -goal);;
143 rightSpeed = next(rightSpeed, goal);
144 break;
145 }
147 default: {
148 break;
149 }
150 }
152 /* clip speeds to allowable range */
153 leftSpeed = clip(leftSpeed);
154 rightSpeed = clip(rightSpeed);
156 /* set motor speeds */
157 ZumoMotors::setLeftSpeed(leftSpeed);
158 ZumoMotors::setRightSpeed(rightSpeed);
160 /* sleep for 1 ms (so duration is in milliseconds) */
161 delay(1);
162 }
163 }
165 /*
166 * ======== next ========
167 * Compute the next motor speed value given the cur speed and a new goal
168 */
169 static int next(int cur, int goal)
170 {
171 int tmp = (goal - cur) * 0.0625f + cur;
172 return (tmp == cur ? goal : tmp);
173 }