Mine, not yours justin
Dependencies: HMC6352 USBHost mbed-rtos mbed
Fork of Project by
RobotControl.h@4:b2a30c313297, 2014-04-25 (annotated)
- Committer:
- ganeshsubram1
- Date:
- Fri Apr 25 23:08:01 2014 +0000
- Revision:
- 4:b2a30c313297
- Parent:
- 3:6a7620e9abd9
First Publish. Working 90 degree turning calibration and execution, just need a good power supply to fully test.
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 | //******************************************* |
ganeshsubram1 | 3:6a7620e9abd9 | 4 | |
Strikewolf | 0:b6fd1c37944a | 5 | #include "HMC6352.h" |
ganeshsubram1 | 3:6a7620e9abd9 | 6 | #include "PositionSystem.h" |
ganeshsubram1 | 3:6a7620e9abd9 | 7 | #include "SonarSystem.h" |
ganeshsubram1 | 3:6a7620e9abd9 | 8 | #include <math.h> |
Strikewolf | 0:b6fd1c37944a | 9 | |
ganeshsubram1 | 3:6a7620e9abd9 | 10 | #define ROTATE_90_TIME 1330 // Time for bot to rotate 90 degrees at PTURN_SPEED in ms |
ganeshsubram1 | 3:6a7620e9abd9 | 11 | |
Strikewolf | 2:56eb726bdb0d | 12 | //Radio Calibration Signals - Need to implement calibration routine... |
Strikewolf | 2:56eb726bdb0d | 13 | #define CHAN3_MAX 2060 //Throttle Hi |
Strikewolf | 2:56eb726bdb0d | 14 | #define CHAN3_MIN 1150 //Throttle Lo |
Strikewolf | 2:56eb726bdb0d | 15 | #define CHAN2_MAX 1260 //Elevator Hi |
Strikewolf | 2:56eb726bdb0d | 16 | #define CHAN2_MIN 2290 //Elevator Lo |
Strikewolf | 2:56eb726bdb0d | 17 | #define CHAN1_MAX 2160 //Rudder Hi |
Strikewolf | 2:56eb726bdb0d | 18 | #define CHAN1_MIN 1210 //Rudder Lo |
ganeshsubram1 | 3:6a7620e9abd9 | 19 | |
Strikewolf | 0:b6fd1c37944a | 20 | //Gyro |
Strikewolf | 1:41cee26b35cc | 21 | HMC6352 compass(p28, p27); |
ganeshsubram1 | 3:6a7620e9abd9 | 22 | |
Strikewolf | 0:b6fd1c37944a | 23 | //H-bridge |
Strikewolf | 2:56eb726bdb0d | 24 | PwmOut rightMotorPWM(p22); //Channel A |
Strikewolf | 2:56eb726bdb0d | 25 | PwmOut leftMotorPWM(p21); //Channel B |
Strikewolf | 0:b6fd1c37944a | 26 | DigitalOut leftMotor1(p23); |
Strikewolf | 0:b6fd1c37944a | 27 | DigitalOut leftMotor2(p24); |
Strikewolf | 0:b6fd1c37944a | 28 | DigitalOut rightMotor1(p25); |
Strikewolf | 0:b6fd1c37944a | 29 | DigitalOut rightMotor2(p26); |
Strikewolf | 0:b6fd1c37944a | 30 | |
ganeshsubram1 | 3:6a7620e9abd9 | 31 | float sampleCompass(int sample_size) { |
ganeshsubram1 | 3:6a7620e9abd9 | 32 | float c = 0; |
ganeshsubram1 | 3:6a7620e9abd9 | 33 | for(int i = 0; i < sample_size; i++) |
ganeshsubram1 | 3:6a7620e9abd9 | 34 | c += compass.sample() / 10.0; |
ganeshsubram1 | 3:6a7620e9abd9 | 35 | c = c / (double) sample_size; |
ganeshsubram1 | 3:6a7620e9abd9 | 36 | return c; |
Strikewolf | 2:56eb726bdb0d | 37 | } |
ganeshsubram1 | 3:6a7620e9abd9 | 38 | |
Strikewolf | 0:b6fd1c37944a | 39 | //Non-feedback corrected 'dumb' driving |
ganeshsubram1 | 3:6a7620e9abd9 | 40 | void setMove(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 | } |
ganeshsubram1 | 3:6a7620e9abd9 | 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 | } |
ganeshsubram1 | 3:6a7620e9abd9 | 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 | } |
ganeshsubram1 | 3:6a7620e9abd9 | 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 | |
ganeshsubram1 | 3:6a7620e9abd9 | 86 | // calibrate 90 degree turns |
ganeshsubram1 | 3:6a7620e9abd9 | 87 | void turnCalibrate() { |
Strikewolf | 2:56eb726bdb0d | 88 | |
ganeshsubram1 | 3:6a7620e9abd9 | 89 | // tell positioning system to set the turn |
ganeshsubram1 | 3:6a7620e9abd9 | 90 | // calibration variable, PTURN_RIGHT |
ganeshsubram1 | 3:6a7620e9abd9 | 91 | PFlag = PFLAG_CALIB; |
ganeshsubram1 | 3:6a7620e9abd9 | 92 | |
ganeshsubram1 | 3:6a7620e9abd9 | 93 | // start turning hardcoded speed PTURN_SPEED |
ganeshsubram1 | 3:6a7620e9abd9 | 94 | setTurnRight(PTURN_SPEED); |
ganeshsubram1 | 3:6a7620e9abd9 | 95 | |
ganeshsubram1 | 3:6a7620e9abd9 | 96 | // wait until ROTATE_90_TIME has passed. This should |
ganeshsubram1 | 3:6a7620e9abd9 | 97 | // be the time to complete a 90 degree turn |
ganeshsubram1 | 3:6a7620e9abd9 | 98 | wait_ms(ROTATE_90_TIME); |
Strikewolf | 1:41cee26b35cc | 99 | stop(); |
ganeshsubram1 | 3:6a7620e9abd9 | 100 | |
ganeshsubram1 | 3:6a7620e9abd9 | 101 | // done calibrating, turn positioning system back on |
ganeshsubram1 | 3:6a7620e9abd9 | 102 | PFlag = PFLAG_ON; |
Strikewolf | 1:41cee26b35cc | 103 | } |
Strikewolf | 2:56eb726bdb0d | 104 | |
ganeshsubram1 | 3:6a7620e9abd9 | 105 | // ------------------------------------------------------------------- |
ganeshsubram1 | 3:6a7620e9abd9 | 106 | |
Strikewolf | 2:56eb726bdb0d | 107 | //Turn right about the center |
ganeshsubram1 | 3:6a7620e9abd9 | 108 | void turnRight(int delta_degrees) |
Strikewolf | 2:56eb726bdb0d | 109 | { |
ganeshsubram1 | 3:6a7620e9abd9 | 110 | pc.printf("Turnleft: \n\r"); |
ganeshsubram1 | 3:6a7620e9abd9 | 111 | |
ganeshsubram1 | 3:6a7620e9abd9 | 112 | // turn positioning system off during turns since |
ganeshsubram1 | 3:6a7620e9abd9 | 113 | // no x and y movements |
ganeshsubram1 | 3:6a7620e9abd9 | 114 | PFlag = PFLAG_OFF; |
Strikewolf | 2:56eb726bdb0d | 115 | |
ganeshsubram1 | 3:6a7620e9abd9 | 116 | // store new orientation after this turn executes |
ganeshsubram1 | 3:6a7620e9abd9 | 117 | t_position += 90; |
ganeshsubram1 | 3:6a7620e9abd9 | 118 | t_position %= 360; |
ganeshsubram1 | 3:6a7620e9abd9 | 119 | |
ganeshsubram1 | 3:6a7620e9abd9 | 120 | // initialize turn variables |
ganeshsubram1 | 3:6a7620e9abd9 | 121 | turnInit(); |
Strikewolf | 2:56eb726bdb0d | 122 | |
ganeshsubram1 | 3:6a7620e9abd9 | 123 | // start turning right |
ganeshsubram1 | 3:6a7620e9abd9 | 124 | setTurnRight(PTURN_SPEED); |
ganeshsubram1 | 3:6a7620e9abd9 | 125 | |
ganeshsubram1 | 3:6a7620e9abd9 | 126 | // read mouse sensor until 90 degress has completed |
ganeshsubram1 | 3:6a7620e9abd9 | 127 | while(pturn_x > PTURN_RIGHT) { |
ganeshsubram1 | 3:6a7620e9abd9 | 128 | pc.printf("%f, %f\n\r", pturn_x, pturn_y); |
Strikewolf | 2:56eb726bdb0d | 129 | } |
ganeshsubram1 | 3:6a7620e9abd9 | 130 | |
ganeshsubram1 | 3:6a7620e9abd9 | 131 | // stop turning |
Strikewolf | 2:56eb726bdb0d | 132 | stop(); |
ganeshsubram1 | 3:6a7620e9abd9 | 133 | |
ganeshsubram1 | 3:6a7620e9abd9 | 134 | // turn positioning system back on |
ganeshsubram1 | 3:6a7620e9abd9 | 135 | PFlag = PFLAG_ON; |
Strikewolf | 2:56eb726bdb0d | 136 | } |
Strikewolf | 2:56eb726bdb0d | 137 | |
Strikewolf | 2:56eb726bdb0d | 138 | //Drive straight with compass correction |
ganeshsubram1 | 3:6a7620e9abd9 | 139 | void move(float speed, bool reverse) { |
ganeshsubram1 | 3:6a7620e9abd9 | 140 | |
ganeshsubram1 | 3:6a7620e9abd9 | 141 | // begin moving |
ganeshsubram1 | 3:6a7620e9abd9 | 142 | setMove(speed, reverse); |
Strikewolf | 2:56eb726bdb0d | 143 | |
ganeshsubram1 | 3:6a7620e9abd9 | 144 | // blocking call |
ganeshsubram1 | 3:6a7620e9abd9 | 145 | while(1) { |
ganeshsubram1 | 3:6a7620e9abd9 | 146 | |
ganeshsubram1 | 3:6a7620e9abd9 | 147 | // read all sonar sensors |
ganeshsubram1 | 3:6a7620e9abd9 | 148 | float s1 = sonar1.read() * 100; |
ganeshsubram1 | 3:6a7620e9abd9 | 149 | float s2 = sonar2.read() * 100; |
ganeshsubram1 | 3:6a7620e9abd9 | 150 | float s3 = sonar3.read() * 100; |
ganeshsubram1 | 3:6a7620e9abd9 | 151 | |
ganeshsubram1 | 3:6a7620e9abd9 | 152 | // check if obstacle is near and adjust |
ganeshsubram1 | 3:6a7620e9abd9 | 153 | if(s2 < SONAR_STOP) { |
ganeshsubram1 | 3:6a7620e9abd9 | 154 | pc.printf("%.2f, %.2f, %.2f\n\r", s1, s2, s3); |
ganeshsubram1 | 3:6a7620e9abd9 | 155 | if(s1 >= SONAR_STOP) { |
ganeshsubram1 | 3:6a7620e9abd9 | 156 | //turnLeft(90); |
ganeshsubram1 | 3:6a7620e9abd9 | 157 | } else if(s3 >= SONAR_STOP) { |
ganeshsubram1 | 3:6a7620e9abd9 | 158 | turnRight(90); |
ganeshsubram1 | 3:6a7620e9abd9 | 159 | } else { |
ganeshsubram1 | 3:6a7620e9abd9 | 160 | pc.printf("IM STUCK HALP\n\t"); |
ganeshsubram1 | 3:6a7620e9abd9 | 161 | } |
ganeshsubram1 | 3:6a7620e9abd9 | 162 | } |
ganeshsubram1 | 3:6a7620e9abd9 | 163 | |
Strikewolf | 2:56eb726bdb0d | 164 | } |
ganeshsubram1 | 3:6a7620e9abd9 | 165 | } |
ganeshsubram1 | 3:6a7620e9abd9 | 166 | |
ganeshsubram1 | 3:6a7620e9abd9 | 167 |