Ganesh Subramaniam / Mbed 2 deprecated Project

Mine, not yours justin

Dependencies:   HMC6352 USBHost mbed-rtos mbed

Fork of Project by Justin Eng

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
Child:
3:6a7620e9abd9
Complete initial working RobotControl drive functions using the honeywell compass module. Also added in emergency stop function.

Who changed what in which revision?

UserRevisionLine numberNew 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 }