add ability to build and upload sketches from the bash command line (via beta ino2cpp...
[zumo-cc3200/zumo-cc3200.git] / src / Energia / ZumoTest / motorLoop.ino
1 /*
2  *  ======== motorLoop ========
3  *  This sketch controls the motors on the Zumo by polling a global
4  *  variable, motorCmd, 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  *      'r' - raw reads from motorLeft and motorRight
11  *      ' ' - stop
12  *
13  *  Other sketches or interrupts can control the zumo by simply writing the
14  *  desired command to motorCmd.
15  */
16 #include <Energia.h>
18 #include "ZumoMotors.h"
20 #define PERIOD  1           /* period of motor control updates */
22 char motorCmd = ' ';       /* current motor drive command */
23 int motorLeft = 0;
24 int motorRight = 0;
26 static ZumoMotors motors;   /* Zumo motor driver provided by Pololu */
28 static int clip(int speed);
29 static void drive(char wasdr, int goal, unsigned int duration);
31 /*
32  *  ======== motorSetup ========
33  */
34 void motorSetup(void)
35 {
36     Serial.println("motorSetup ...");
37     /* initialize the Pololu driver motor library */
38     motors.setRightSpeed(0);
39     motors.setLeftSpeed(0);
41     /* setup an LED to indcate forward/backward movement or turning */
42     pinMode(MAIN_LED_PIN, OUTPUT);
43     Serial.println("motorSetup done.");
44 }
46 /*
47  *  ======== motorLoop ========
48  */
49 void motorLoop(void)
50 {
51     /* state used to blink LED */
52     static unsigned count = 0;
53     static char state = 0;
54     
55     switch (motorCmd) {
56         case 's':
57         case 'w': {
58             /* illuminate LED while driving */
59             digitalWrite(MAIN_LED_PIN, HIGH);
60             drive(motorCmd, 200, PERIOD);
61             break;
62         }
64         case 'd':
65         case 'a': {
66             /* blink LED while turning */
67             if (count == ((count / 100) * 100)) {
68                 state ^= 1;
69                 digitalWrite(MAIN_LED_PIN, state);
70             }   
71             drive(motorCmd, 100, PERIOD);
72             break;
73         }
74         
75         case 'r': {
76             /* blink LED while turning */
77             if (count == ((count / 50) * 50)) {
78                 state ^= 1;
79                 digitalWrite(MAIN_LED_PIN, state);
80             }  
81             drive('r', 0, PERIOD);
82             break;
83         }
85         default: {
86             /* turn off LED while stopped */
87             motorCmd = ' ';
88             digitalWrite(MAIN_LED_PIN, LOW);
89             drive(' ', 0, 10);
90             break;
91         }
92     }
94     count++;
95 }
97 /*
98  *  ======== clip ========
99  */
100 static int clip(int speed)
102     if (speed < -400) {
103         speed = -400;
104     }
105     else if (speed > 400) {
106         speed = 400;
107     }
108     return (speed);
111 /*
112  *  ======== drive ========
113  */
114 #define P 0.9375f
115 static void drive(char wasdr, int goal, unsigned int duration)
117     static int leftSpeed = 0;
118     static int rightSpeed = 0;
119     
120     while (duration > 0) {
121         duration--;
123         /* gradually adjust curent speeds to goal */
124         switch (wasdr) {
125             case ' ': { /* stop */
126                 leftSpeed = (int)(P * leftSpeed);
127                 rightSpeed = (int)(P * rightSpeed);
128                 break;
129             }
130             case 'w': { /* forward */
131                 leftSpeed = (leftSpeed - goal) * P + goal;
132                 rightSpeed = (rightSpeed - goal) * P + goal;
133                 break;
134             }
136             case 's': { /* backward */
137                 leftSpeed = (leftSpeed + goal) * P - goal;
138                 rightSpeed = (rightSpeed + goal) * P - goal;
139                 break;
140             }
142             case 'd': { /* turn right */
143                 leftSpeed = (leftSpeed - goal) * P + goal;
144                 rightSpeed = (rightSpeed + goal) * P - goal;
145                 break;
146             }
148             case 'a': { /* turn left */
149                 leftSpeed = (leftSpeed + goal) * P - goal;
150                 rightSpeed = (rightSpeed - goal) * P + goal;
151                 break;
152             }
153             
154             case 'r': {
155                leftSpeed = (leftSpeed - motorLeft) * P + motorLeft;
156                rightSpeed = (rightSpeed - motorRight) * P + motorRight;
157                break; 
158             }
160             default: {
161                 break;
162             }
163         }
165         /* clip speeds to allowable range */
166         leftSpeed = clip(leftSpeed);
167         rightSpeed = clip(rightSpeed);
168     
169         /* set motor speeds */
170         motors.setLeftSpeed(leftSpeed);
171         motors.setRightSpeed(rightSpeed);
173         /* sleep for 1 ms (so duration is in milliseconds) */
174         delay(1);
175     }