![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
StingerBot Project
Dependencies: mbed HMC6352 mbed-rtos
RobotControl.h@2:56eb726bdb0d, 2014-04-22 (annotated)
- Committer:
- Strikewolf
- Date:
- Tue Apr 22 23:16:32 2014 +0000
- Revision:
- 2:56eb726bdb0d
- Parent:
- 1:41cee26b35cc
Complete initial working RobotControl drive functions using the honeywell compass module. Also added in emergency stop function.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Strikewolf | 0:b6fd1c37944a | 1 | //******************************************* |
Strikewolf | 0:b6fd1c37944a | 2 | //* Robot motor control and drive functions * |
Strikewolf | 0:b6fd1c37944a | 3 | //******************************************* |
Strikewolf | 0:b6fd1c37944a | 4 | |
Strikewolf | 0:b6fd1c37944a | 5 | #include "HMC6352.h" |
Strikewolf | 0:b6fd1c37944a | 6 | |
Strikewolf | 2:56eb726bdb0d | 7 | //Radio Calibration Signals - Need to implement calibration routine... |
Strikewolf | 2:56eb726bdb0d | 8 | #define CHAN3_MAX 2060 //Throttle Hi |
Strikewolf | 2:56eb726bdb0d | 9 | #define CHAN3_MIN 1150 //Throttle Lo |
Strikewolf | 2:56eb726bdb0d | 10 | #define CHAN2_MAX 1260 //Elevator Hi |
Strikewolf | 2:56eb726bdb0d | 11 | #define CHAN2_MIN 2290 //Elevator Lo |
Strikewolf | 2:56eb726bdb0d | 12 | #define CHAN1_MAX 2160 //Rudder Hi |
Strikewolf | 2:56eb726bdb0d | 13 | #define CHAN1_MIN 1210 //Rudder Lo |
Strikewolf | 2:56eb726bdb0d | 14 | |
Strikewolf | 2:56eb726bdb0d | 15 | //Debug |
Strikewolf | 2:56eb726bdb0d | 16 | Serial pc(USBTX, USBRX); |
Strikewolf | 2:56eb726bdb0d | 17 | |
Strikewolf | 0:b6fd1c37944a | 18 | //Gyro |
Strikewolf | 1:41cee26b35cc | 19 | HMC6352 compass(p28, p27); |
Strikewolf | 0:b6fd1c37944a | 20 | |
Strikewolf | 0:b6fd1c37944a | 21 | //H-bridge |
Strikewolf | 2:56eb726bdb0d | 22 | PwmOut rightMotorPWM(p22); //Channel A |
Strikewolf | 2:56eb726bdb0d | 23 | PwmOut leftMotorPWM(p21); //Channel B |
Strikewolf | 0:b6fd1c37944a | 24 | DigitalOut leftMotor1(p23); |
Strikewolf | 0:b6fd1c37944a | 25 | DigitalOut leftMotor2(p24); |
Strikewolf | 0:b6fd1c37944a | 26 | DigitalOut rightMotor1(p25); |
Strikewolf | 0:b6fd1c37944a | 27 | DigitalOut rightMotor2(p26); |
Strikewolf | 0:b6fd1c37944a | 28 | |
Strikewolf | 2:56eb726bdb0d | 29 | //Compass sampling functions - TBA if compass keeps freaking out |
Strikewolf | 2:56eb726bdb0d | 30 | int prev_sample, cur_sample; |
Strikewolf | 2:56eb726bdb0d | 31 | void prepCompass() { |
Strikewolf | 2:56eb726bdb0d | 32 | int i; |
Strikewolf | 2:56eb726bdb0d | 33 | for (i = 0; i <= 4; i++) { |
Strikewolf | 2:56eb726bdb0d | 34 | //Chew through a few samples, the compass seems to return "stale" values if unsampled for awhile |
Strikewolf | 2:56eb726bdb0d | 35 | compass.sample(); |
Strikewolf | 2:56eb726bdb0d | 36 | } |
Strikewolf | 2:56eb726bdb0d | 37 | } |
Strikewolf | 2:56eb726bdb0d | 38 | |
Strikewolf | 0:b6fd1c37944a | 39 | //Non-feedback corrected 'dumb' driving |
Strikewolf | 0:b6fd1c37944a | 40 | void setDriveStraight(float speed, bool reverse) { |
Strikewolf | 0:b6fd1c37944a | 41 | //Set speed |
Strikewolf | 0:b6fd1c37944a | 42 | rightMotorPWM = speed; |
Strikewolf | 0:b6fd1c37944a | 43 | leftMotorPWM = speed; |
Strikewolf | 0:b6fd1c37944a | 44 | |
Strikewolf | 0:b6fd1c37944a | 45 | //Set motor function |
Strikewolf | 0:b6fd1c37944a | 46 | leftMotor1 = (!reverse) ? 0 : 1; |
Strikewolf | 0:b6fd1c37944a | 47 | leftMotor2 = (!reverse) ? 1 : 0; |
Strikewolf | 0:b6fd1c37944a | 48 | rightMotor1 = (!reverse) ? 0 : 1; |
Strikewolf | 0:b6fd1c37944a | 49 | rightMotor2 = (!reverse) ? 1 : 0; |
Strikewolf | 0:b6fd1c37944a | 50 | } |
Strikewolf | 0:b6fd1c37944a | 51 | |
Strikewolf | 2:56eb726bdb0d | 52 | void setTurnLeft(float speed) { |
Strikewolf | 2:56eb726bdb0d | 53 | //Set speed |
Strikewolf | 2:56eb726bdb0d | 54 | rightMotorPWM = speed; |
Strikewolf | 2:56eb726bdb0d | 55 | leftMotorPWM = speed; |
Strikewolf | 2:56eb726bdb0d | 56 | |
Strikewolf | 2:56eb726bdb0d | 57 | //Set motor function |
Strikewolf | 2:56eb726bdb0d | 58 | leftMotor1 = 0; |
Strikewolf | 2:56eb726bdb0d | 59 | leftMotor2 = 1; |
Strikewolf | 2:56eb726bdb0d | 60 | rightMotor1 = 1; |
Strikewolf | 2:56eb726bdb0d | 61 | rightMotor2 = 0; |
Strikewolf | 2:56eb726bdb0d | 62 | } |
Strikewolf | 2:56eb726bdb0d | 63 | |
Strikewolf | 2:56eb726bdb0d | 64 | void setTurnRight(float speed) { |
Strikewolf | 2:56eb726bdb0d | 65 | //Set speed |
Strikewolf | 2:56eb726bdb0d | 66 | rightMotorPWM = speed; |
Strikewolf | 2:56eb726bdb0d | 67 | leftMotorPWM = speed; |
Strikewolf | 2:56eb726bdb0d | 68 | |
Strikewolf | 2:56eb726bdb0d | 69 | //Set motor function |
Strikewolf | 2:56eb726bdb0d | 70 | leftMotor1 = 1; |
Strikewolf | 2:56eb726bdb0d | 71 | leftMotor2 = 0; |
Strikewolf | 2:56eb726bdb0d | 72 | rightMotor1 = 0; |
Strikewolf | 2:56eb726bdb0d | 73 | rightMotor2 = 1; |
Strikewolf | 2:56eb726bdb0d | 74 | } |
Strikewolf | 2:56eb726bdb0d | 75 | |
Strikewolf | 0:b6fd1c37944a | 76 | //Stop with braking |
Strikewolf | 0:b6fd1c37944a | 77 | void stop() { |
Strikewolf | 0:b6fd1c37944a | 78 | rightMotorPWM = 0; |
Strikewolf | 0:b6fd1c37944a | 79 | leftMotorPWM = 0; |
Strikewolf | 0:b6fd1c37944a | 80 | leftMotor1 = 0; |
Strikewolf | 1:41cee26b35cc | 81 | leftMotor2 = 1; |
Strikewolf | 1:41cee26b35cc | 82 | rightMotor1 = 1; |
Strikewolf | 0:b6fd1c37944a | 83 | rightMotor2 = 0; |
Strikewolf | 0:b6fd1c37944a | 84 | } |
Strikewolf | 0:b6fd1c37944a | 85 | |
Strikewolf | 2:56eb726bdb0d | 86 | //Turn left about the center |
Strikewolf | 1:41cee26b35cc | 87 | void centerTurnLeft(int delta_degrees) { |
Strikewolf | 2:56eb726bdb0d | 88 | prepCompass(); |
Strikewolf | 2:56eb726bdb0d | 89 | |
Strikewolf | 2:56eb726bdb0d | 90 | float initial = (compass.sample() / 10); |
Strikewolf | 2:56eb726bdb0d | 91 | float target = initial - delta_degrees; |
Strikewolf | 2:56eb726bdb0d | 92 | double sample; |
Strikewolf | 2:56eb726bdb0d | 93 | bool rollover = false; |
Strikewolf | 2:56eb726bdb0d | 94 | |
Strikewolf | 2:56eb726bdb0d | 95 | if (target < 0) { |
Strikewolf | 2:56eb726bdb0d | 96 | target += 360; |
Strikewolf | 2:56eb726bdb0d | 97 | rollover = true; |
Strikewolf | 2:56eb726bdb0d | 98 | } |
Strikewolf | 2:56eb726bdb0d | 99 | |
Strikewolf | 2:56eb726bdb0d | 100 | pc.printf("Init: %f Target: %f\r\n", initial, target); |
Strikewolf | 2:56eb726bdb0d | 101 | |
Strikewolf | 2:56eb726bdb0d | 102 | //Take care of 360 degree rollover |
Strikewolf | 2:56eb726bdb0d | 103 | if(rollover) { |
Strikewolf | 2:56eb726bdb0d | 104 | pc.printf("begin rollover \r\n"); |
Strikewolf | 2:56eb726bdb0d | 105 | while(true) { |
Strikewolf | 2:56eb726bdb0d | 106 | setTurnLeft(0.6); |
Strikewolf | 2:56eb726bdb0d | 107 | sample = compass.sample() / 10; |
Strikewolf | 2:56eb726bdb0d | 108 | wait(0.02); |
Strikewolf | 2:56eb726bdb0d | 109 | if (sample > 345) |
Strikewolf | 2:56eb726bdb0d | 110 | break; |
Strikewolf | 2:56eb726bdb0d | 111 | } |
Strikewolf | 2:56eb726bdb0d | 112 | pc.printf("rollover complete\r\n"); |
Strikewolf | 2:56eb726bdb0d | 113 | } |
Strikewolf | 2:56eb726bdb0d | 114 | |
Strikewolf | 2:56eb726bdb0d | 115 | //Continue to turn to target |
Strikewolf | 2:56eb726bdb0d | 116 | while (true) { |
Strikewolf | 2:56eb726bdb0d | 117 | setTurnLeft(0.6); |
Strikewolf | 2:56eb726bdb0d | 118 | sample = compass.sample()/10; |
Strikewolf | 2:56eb726bdb0d | 119 | wait(0.02); |
Strikewolf | 2:56eb726bdb0d | 120 | if(sample < target) |
Strikewolf | 2:56eb726bdb0d | 121 | break; |
Strikewolf | 1:41cee26b35cc | 122 | } |
Strikewolf | 1:41cee26b35cc | 123 | stop(); |
Strikewolf | 1:41cee26b35cc | 124 | } |
Strikewolf | 2:56eb726bdb0d | 125 | |
Strikewolf | 2:56eb726bdb0d | 126 | //Turn right about the center |
Strikewolf | 2:56eb726bdb0d | 127 | void centerTurnRight(int delta_degrees) |
Strikewolf | 2:56eb726bdb0d | 128 | { |
Strikewolf | 2:56eb726bdb0d | 129 | prepCompass(); |
Strikewolf | 2:56eb726bdb0d | 130 | |
Strikewolf | 2:56eb726bdb0d | 131 | float initial = (compass.sample() / 10); |
Strikewolf | 2:56eb726bdb0d | 132 | float target = initial + delta_degrees; |
Strikewolf | 2:56eb726bdb0d | 133 | double sample; |
Strikewolf | 2:56eb726bdb0d | 134 | bool rollover = false; |
Strikewolf | 2:56eb726bdb0d | 135 | |
Strikewolf | 2:56eb726bdb0d | 136 | if (target > 360) { |
Strikewolf | 2:56eb726bdb0d | 137 | target -= 360; |
Strikewolf | 2:56eb726bdb0d | 138 | rollover = true; |
Strikewolf | 2:56eb726bdb0d | 139 | } |
Strikewolf | 2:56eb726bdb0d | 140 | |
Strikewolf | 2:56eb726bdb0d | 141 | pc.printf("Init: %f Target: %f\r\n", initial, target); |
Strikewolf | 2:56eb726bdb0d | 142 | |
Strikewolf | 2:56eb726bdb0d | 143 | //Take care of 360 degree rollover |
Strikewolf | 2:56eb726bdb0d | 144 | if(rollover) { |
Strikewolf | 2:56eb726bdb0d | 145 | pc.printf("begin rollover \r\n"); |
Strikewolf | 2:56eb726bdb0d | 146 | |
Strikewolf | 2:56eb726bdb0d | 147 | while(true) { |
Strikewolf | 2:56eb726bdb0d | 148 | setTurnRight(0.6); |
Strikewolf | 2:56eb726bdb0d | 149 | sample = compass.sample() / 10; |
Strikewolf | 2:56eb726bdb0d | 150 | wait(0.02); |
Strikewolf | 2:56eb726bdb0d | 151 | if (sample > 0 && sample < 10) |
Strikewolf | 2:56eb726bdb0d | 152 | break; |
Strikewolf | 2:56eb726bdb0d | 153 | } |
Strikewolf | 2:56eb726bdb0d | 154 | pc.printf("rollover complete\r\n"); |
Strikewolf | 2:56eb726bdb0d | 155 | } |
Strikewolf | 2:56eb726bdb0d | 156 | |
Strikewolf | 2:56eb726bdb0d | 157 | //Continue to turn to target |
Strikewolf | 2:56eb726bdb0d | 158 | while (true) { |
Strikewolf | 2:56eb726bdb0d | 159 | setTurnRight(0.6); |
Strikewolf | 2:56eb726bdb0d | 160 | sample = compass.sample()/10; |
Strikewolf | 2:56eb726bdb0d | 161 | wait(0.02); |
Strikewolf | 2:56eb726bdb0d | 162 | if(sample > target) |
Strikewolf | 2:56eb726bdb0d | 163 | break; |
Strikewolf | 2:56eb726bdb0d | 164 | } |
Strikewolf | 2:56eb726bdb0d | 165 | stop(); |
Strikewolf | 2:56eb726bdb0d | 166 | } |
Strikewolf | 2:56eb726bdb0d | 167 | |
Strikewolf | 2:56eb726bdb0d | 168 | //Drive straight with compass correction |
Strikewolf | 2:56eb726bdb0d | 169 | void compassDriveStraight(float speed, bool reverse, int ms) { |
Strikewolf | 2:56eb726bdb0d | 170 | Timer timer; |
Strikewolf | 2:56eb726bdb0d | 171 | double sample; |
Strikewolf | 2:56eb726bdb0d | 172 | |
Strikewolf | 2:56eb726bdb0d | 173 | setDriveStraight(speed, reverse); |
Strikewolf | 2:56eb726bdb0d | 174 | wait(0.2); |
Strikewolf | 2:56eb726bdb0d | 175 | //really hacky way to compensate for motor EM interference |
Strikewolf | 2:56eb726bdb0d | 176 | prepCompass(); |
Strikewolf | 2:56eb726bdb0d | 177 | float initial = compass.sample() / 10; |
Strikewolf | 2:56eb726bdb0d | 178 | |
Strikewolf | 2:56eb726bdb0d | 179 | //Really hacky way to deal with 360 rollover |
Strikewolf | 2:56eb726bdb0d | 180 | if (initial + 6 > 360) |
Strikewolf | 2:56eb726bdb0d | 181 | centerTurnLeft(7); |
Strikewolf | 2:56eb726bdb0d | 182 | if (initial - 6 < 0) |
Strikewolf | 2:56eb726bdb0d | 183 | centerTurnRight(7); |
Strikewolf | 2:56eb726bdb0d | 184 | |
Strikewolf | 2:56eb726bdb0d | 185 | timer.start(); |
Strikewolf | 2:56eb726bdb0d | 186 | while (timer.read_ms() < ms) { |
Strikewolf | 2:56eb726bdb0d | 187 | sample = compass.sample() / 10; |
Strikewolf | 2:56eb726bdb0d | 188 | pc.printf("%f :::", sample); |
Strikewolf | 2:56eb726bdb0d | 189 | |
Strikewolf | 2:56eb726bdb0d | 190 | if (sample > initial + 3) { |
Strikewolf | 2:56eb726bdb0d | 191 | pc.printf("Steer Left\r\n"); |
Strikewolf | 2:56eb726bdb0d | 192 | leftMotorPWM = speed - 0.1; |
Strikewolf | 2:56eb726bdb0d | 193 | rightMotorPWM = speed + 0.1; |
Strikewolf | 2:56eb726bdb0d | 194 | leftMotor1 = 0; |
Strikewolf | 2:56eb726bdb0d | 195 | leftMotor2 = 1; |
Strikewolf | 2:56eb726bdb0d | 196 | rightMotor1 = 0; |
Strikewolf | 2:56eb726bdb0d | 197 | rightMotor2 = 1; |
Strikewolf | 2:56eb726bdb0d | 198 | } else if (sample < initial - 3) { |
Strikewolf | 2:56eb726bdb0d | 199 | pc.printf("Steer Right \r\n"); |
Strikewolf | 2:56eb726bdb0d | 200 | leftMotorPWM = speed + 0.1; |
Strikewolf | 2:56eb726bdb0d | 201 | rightMotorPWM = speed - 0.1; |
Strikewolf | 2:56eb726bdb0d | 202 | leftMotor1 = 0; |
Strikewolf | 2:56eb726bdb0d | 203 | leftMotor2 = 1; |
Strikewolf | 2:56eb726bdb0d | 204 | rightMotor1 = 0; |
Strikewolf | 2:56eb726bdb0d | 205 | rightMotor2 = 1; |
Strikewolf | 2:56eb726bdb0d | 206 | } else { |
Strikewolf | 2:56eb726bdb0d | 207 | pc.printf("Straight\r\n"); |
Strikewolf | 2:56eb726bdb0d | 208 | setDriveStraight(speed, reverse); |
Strikewolf | 2:56eb726bdb0d | 209 | } |
Strikewolf | 2:56eb726bdb0d | 210 | } |
Strikewolf | 2:56eb726bdb0d | 211 | stop(); |
Strikewolf | 2:56eb726bdb0d | 212 | } |