Ganesh Subramaniam / Mbed 2 deprecated Project

Mine, not yours justin

Dependencies:   HMC6352 USBHost mbed-rtos mbed

Fork of Project by Justin Eng

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?

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