instructions.h (988B)
1 #include <Servo.h> 2 #include "util.h" 3 4 enum DIRECTIONS { 5 FWD = 1, 6 BWD = -1, 7 LFT = -2, 8 RGT = 2 9 }; 10 11 typedef struct instructions { 12 int dir, unit; 13 struct instructions *next; 14 } instructions; 15 16 void 17 stepMove(int dir) { 18 servoA.write(SERVOA_ZERO + (40 * dir)); 19 servoB.write(SERVOA_ZERO - (40 * dir)); 20 delay(10); 21 servoA.write(SERVOA_ZERO); 22 servoB.write(SERVOA_ZERO); 23 } 24 25 void 26 stepTurn(int dir) { 27 servoA.write(SERVOA_ZERO + (10 * dir)); 28 servoB.write(SERVOA_ZERO - (10 * -dir)); 29 delay(10); 30 servoA.write(SERVOA_ZERO); 31 servoB.write(SERVOA_ZERO); 32 } 33 34 void 35 rotate(int x) { 36 int dir = (x < 0) ? -1 : 1; 37 x = (x < 0) ? -x : x; 38 39 int y = (int)(1.666666 * (float)x); 40 41 for (int i = 0; i < y; i++) 42 stepTurn(dir); 43 } 44 45 void 46 runInstructions(instructions *ins) { 47 switch (ins->dir) { 48 case FWD: 49 case BWD: 50 for (int i = 0; i < ins->unit; i++) 51 stepMove(ins->dir); 52 break; 53 case LFT: 54 case RGT: 55 rotate(ins->unit * (ins->dir / 2)); 56 break; 57 } 58 if (ins->next) 59 runInstructions(ins->next); 60 }