Justin Eng / Mbed 2 deprecated RoboticsCatAndMouse_AutonomousDriver

Spring 2014, ECE 4180 project, Georgia Institute of Technolgoy. This is the autonomous driver program for the Robotics Cat and Mouse program.

Dependencies:   IMUfilter ADXL345_I2C mbed ITG3200 USBHost mbed-rtos

HumanControl.h@1:dacf7db790f6, 2014-04-30 (annotated)

Committer:
Strikewolf
Date:
Wed Apr 30 05:54:17 2014 +0000
Revision:
1:dacf7db790f6
works without any motors

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Strikewolf 1:dacf7db790f6 1 //Radio Calibration Signals - Need to implement calibration routine...
Strikewolf 1:dacf7db790f6 2 #define CHAN2_MID 1470 //Elevator Midpoint
Strikewolf 1:dacf7db790f6 3 #define CHAN1_MID 1470 //Rudder Midpoint
Strikewolf 1:dacf7db790f6 4 #define DEADZONE 105
Strikewolf 1:dacf7db790f6 5 #define MAXMIN_OFFSET 400
Strikewolf 1:dacf7db790f6 6
Strikewolf 1:dacf7db790f6 7 //Drive Modes
Strikewolf 1:dacf7db790f6 8 typedef enum {FORWARD, REVERSE, STOP, CENTERLEFT, CENTERRIGHT} DRIVEMODE;
Strikewolf 1:dacf7db790f6 9 DRIVEMODE mode;
Strikewolf 1:dacf7db790f6 10 float xpwm, ypwm;
Strikewolf 1:dacf7db790f6 11
Strikewolf 1:dacf7db790f6 12 //H-bridge
Strikewolf 1:dacf7db790f6 13 PwmOut rightMotorPWM(p22); //Channel A
Strikewolf 1:dacf7db790f6 14 PwmOut leftMotorPWM(p21); //Channel B
Strikewolf 1:dacf7db790f6 15 DigitalOut leftMotor1(p23);
Strikewolf 1:dacf7db790f6 16 DigitalOut leftMotor2(p24);
Strikewolf 1:dacf7db790f6 17 DigitalOut rightMotor1(p25);
Strikewolf 1:dacf7db790f6 18 DigitalOut rightMotor2(p26);
Strikewolf 1:dacf7db790f6 19
Strikewolf 1:dacf7db790f6 20 //Radio
Strikewolf 1:dacf7db790f6 21 DigitalIn rudder(p11);
Strikewolf 1:dacf7db790f6 22 DigitalIn elevator(p12);
Strikewolf 1:dacf7db790f6 23
Strikewolf 1:dacf7db790f6 24 void setTurnLeft(float speed)
Strikewolf 1:dacf7db790f6 25 {
Strikewolf 1:dacf7db790f6 26 //Set speed
Strikewolf 1:dacf7db790f6 27 rightMotorPWM = speed;
Strikewolf 1:dacf7db790f6 28 leftMotorPWM = speed;
Strikewolf 1:dacf7db790f6 29
Strikewolf 1:dacf7db790f6 30 //Set motor function
Strikewolf 1:dacf7db790f6 31 leftMotor1 = 0;
Strikewolf 1:dacf7db790f6 32 leftMotor2 = 1;
Strikewolf 1:dacf7db790f6 33 rightMotor1 = 1;
Strikewolf 1:dacf7db790f6 34 rightMotor2 = 0;
Strikewolf 1:dacf7db790f6 35 }
Strikewolf 1:dacf7db790f6 36
Strikewolf 1:dacf7db790f6 37 void setTurnRight(float speed)
Strikewolf 1:dacf7db790f6 38 {
Strikewolf 1:dacf7db790f6 39 //Set speed
Strikewolf 1:dacf7db790f6 40 rightMotorPWM = speed;
Strikewolf 1:dacf7db790f6 41 leftMotorPWM = speed;
Strikewolf 1:dacf7db790f6 42
Strikewolf 1:dacf7db790f6 43 //Set motor function
Strikewolf 1:dacf7db790f6 44 leftMotor1 = 1;
Strikewolf 1:dacf7db790f6 45 leftMotor2 = 0;
Strikewolf 1:dacf7db790f6 46 rightMotor1 = 0;
Strikewolf 1:dacf7db790f6 47 rightMotor2 = 1;
Strikewolf 1:dacf7db790f6 48 }
Strikewolf 1:dacf7db790f6 49
Strikewolf 1:dacf7db790f6 50 //Stop with braking
Strikewolf 1:dacf7db790f6 51 void stop()
Strikewolf 1:dacf7db790f6 52 {
Strikewolf 1:dacf7db790f6 53 rightMotorPWM = 0;
Strikewolf 1:dacf7db790f6 54 leftMotorPWM = 0;
Strikewolf 1:dacf7db790f6 55 leftMotor1 = 0;
Strikewolf 1:dacf7db790f6 56 leftMotor2 = 1;
Strikewolf 1:dacf7db790f6 57 rightMotor1 = 1;
Strikewolf 1:dacf7db790f6 58 rightMotor2 = 0;
Strikewolf 1:dacf7db790f6 59 }
Strikewolf 1:dacf7db790f6 60
Strikewolf 1:dacf7db790f6 61
Strikewolf 1:dacf7db790f6 62 int getRadioX()
Strikewolf 1:dacf7db790f6 63 {
Strikewolf 1:dacf7db790f6 64 Timer timer;
Strikewolf 1:dacf7db790f6 65 timer.reset();
Strikewolf 1:dacf7db790f6 66 int dur;
Strikewolf 1:dacf7db790f6 67 while(!rudder && !gameOver);
Strikewolf 1:dacf7db790f6 68 timer.start();
Strikewolf 1:dacf7db790f6 69 while(rudder && !gameOver);
Strikewolf 1:dacf7db790f6 70 dur = timer.read_us();
Strikewolf 1:dacf7db790f6 71 return dur;
Strikewolf 1:dacf7db790f6 72 }
Strikewolf 1:dacf7db790f6 73
Strikewolf 1:dacf7db790f6 74 int getRadioY()
Strikewolf 1:dacf7db790f6 75 {
Strikewolf 1:dacf7db790f6 76 Timer timer;
Strikewolf 1:dacf7db790f6 77 timer.reset();
Strikewolf 1:dacf7db790f6 78 int dur;
Strikewolf 1:dacf7db790f6 79 while(!elevator && !gameOver);
Strikewolf 1:dacf7db790f6 80 timer.start();
Strikewolf 1:dacf7db790f6 81 while(elevator && !gameOver);
Strikewolf 1:dacf7db790f6 82 dur = timer.read_us();
Strikewolf 1:dacf7db790f6 83 return dur;
Strikewolf 1:dacf7db790f6 84 }
Strikewolf 1:dacf7db790f6 85
Strikewolf 1:dacf7db790f6 86 //Primary function for setting motors for radio driving
Strikewolf 1:dacf7db790f6 87 void radioDrive()
Strikewolf 1:dacf7db790f6 88 {
Strikewolf 1:dacf7db790f6 89 __disable_irq(); // disable interrupts
Strikewolf 1:dacf7db790f6 90 int radioY = getRadioY();
Strikewolf 1:dacf7db790f6 91 int radioX = getRadioX();
Strikewolf 1:dacf7db790f6 92 __enable_irq(); // enable interrupts
Strikewolf 1:dacf7db790f6 93
Strikewolf 1:dacf7db790f6 94 //Identify drive mode from radio values
Strikewolf 1:dacf7db790f6 95 if (radioY < CHAN2_MID - DEADZONE)
Strikewolf 1:dacf7db790f6 96 mode = FORWARD;
Strikewolf 1:dacf7db790f6 97 if (radioY > CHAN2_MID + DEADZONE)
Strikewolf 1:dacf7db790f6 98 mode = REVERSE;
Strikewolf 1:dacf7db790f6 99 if (radioY < CHAN2_MID + DEADZONE && radioY > CHAN2_MID - DEADZONE &&
Strikewolf 1:dacf7db790f6 100 radioX < CHAN1_MID - DEADZONE)
Strikewolf 1:dacf7db790f6 101 mode = CENTERLEFT;
Strikewolf 1:dacf7db790f6 102 if (radioY < CHAN2_MID + DEADZONE && radioY > CHAN2_MID - DEADZONE &&
Strikewolf 1:dacf7db790f6 103 radioX > CHAN1_MID + DEADZONE)
Strikewolf 1:dacf7db790f6 104 mode = CENTERRIGHT;
Strikewolf 1:dacf7db790f6 105 if (radioY < CHAN2_MID + DEADZONE && radioY > CHAN2_MID - DEADZONE &&
Strikewolf 1:dacf7db790f6 106 radioX < CHAN1_MID + DEADZONE && radioX > CHAN1_MID - DEADZONE)
Strikewolf 1:dacf7db790f6 107 mode = STOP;
Strikewolf 1:dacf7db790f6 108
Strikewolf 1:dacf7db790f6 109 //Normalize values for PWM magnitude
Strikewolf 1:dacf7db790f6 110 xpwm = abs((float)radioX - CHAN1_MID) / MAXMIN_OFFSET;
Strikewolf 1:dacf7db790f6 111 ypwm = abs((float)radioY - CHAN2_MID) / MAXMIN_OFFSET;
Strikewolf 1:dacf7db790f6 112
Strikewolf 1:dacf7db790f6 113 //Set Motors Accordingly
Strikewolf 1:dacf7db790f6 114 switch(mode) {
Strikewolf 1:dacf7db790f6 115 case FORWARD:
Strikewolf 1:dacf7db790f6 116 //Handle variable turns
Strikewolf 1:dacf7db790f6 117 if (radioX > CHAN1_MID + DEADZONE) {
Strikewolf 1:dacf7db790f6 118 rightMotorPWM = 0.2;
Strikewolf 1:dacf7db790f6 119 leftMotorPWM = 0.85;
Strikewolf 1:dacf7db790f6 120 } else if (radioX < CHAN1_MID - DEADZONE) {
Strikewolf 1:dacf7db790f6 121 rightMotorPWM = 0.85;
Strikewolf 1:dacf7db790f6 122 leftMotorPWM = 0.2;
Strikewolf 1:dacf7db790f6 123 } else {
Strikewolf 1:dacf7db790f6 124 rightMotorPWM = .85;
Strikewolf 1:dacf7db790f6 125 leftMotorPWM = .85;
Strikewolf 1:dacf7db790f6 126 }
Strikewolf 1:dacf7db790f6 127 leftMotor1 = 0;
Strikewolf 1:dacf7db790f6 128 leftMotor2 = 1;
Strikewolf 1:dacf7db790f6 129 rightMotor1 = 0;
Strikewolf 1:dacf7db790f6 130 rightMotor2 = 1;
Strikewolf 1:dacf7db790f6 131 break;
Strikewolf 1:dacf7db790f6 132
Strikewolf 1:dacf7db790f6 133 case REVERSE:
Strikewolf 1:dacf7db790f6 134 //Handle variable turns
Strikewolf 1:dacf7db790f6 135 if (radioX > CHAN1_MID + DEADZONE) {
Strikewolf 1:dacf7db790f6 136 rightMotorPWM = 0.2;
Strikewolf 1:dacf7db790f6 137 leftMotorPWM = 0.85;
Strikewolf 1:dacf7db790f6 138 } else if (radioX < CHAN1_MID - DEADZONE) {
Strikewolf 1:dacf7db790f6 139 rightMotorPWM = 0.85;
Strikewolf 1:dacf7db790f6 140 leftMotorPWM = 0.2;
Strikewolf 1:dacf7db790f6 141 } else {
Strikewolf 1:dacf7db790f6 142 rightMotorPWM = .85;
Strikewolf 1:dacf7db790f6 143 leftMotorPWM = .85;
Strikewolf 1:dacf7db790f6 144 }
Strikewolf 1:dacf7db790f6 145 leftMotor1 = 1;
Strikewolf 1:dacf7db790f6 146 leftMotor2 = 0;
Strikewolf 1:dacf7db790f6 147 rightMotor1 = 1;
Strikewolf 1:dacf7db790f6 148 rightMotor2 = 0;
Strikewolf 1:dacf7db790f6 149 break;
Strikewolf 1:dacf7db790f6 150
Strikewolf 1:dacf7db790f6 151 case CENTERRIGHT:
Strikewolf 1:dacf7db790f6 152 rightMotorPWM = xpwm;
Strikewolf 1:dacf7db790f6 153 leftMotorPWM = xpwm;
Strikewolf 1:dacf7db790f6 154 leftMotor1 = 1;
Strikewolf 1:dacf7db790f6 155 leftMotor2 = 0;
Strikewolf 1:dacf7db790f6 156 rightMotor1 = 0;
Strikewolf 1:dacf7db790f6 157 rightMotor2 = 1;
Strikewolf 1:dacf7db790f6 158 break;
Strikewolf 1:dacf7db790f6 159
Strikewolf 1:dacf7db790f6 160 case CENTERLEFT:
Strikewolf 1:dacf7db790f6 161 rightMotorPWM = xpwm;
Strikewolf 1:dacf7db790f6 162 leftMotorPWM = xpwm;
Strikewolf 1:dacf7db790f6 163 leftMotor1 = 0;
Strikewolf 1:dacf7db790f6 164 leftMotor2 = 1;
Strikewolf 1:dacf7db790f6 165 rightMotor1 = 1;
Strikewolf 1:dacf7db790f6 166 rightMotor2 = 0;
Strikewolf 1:dacf7db790f6 167 break;
Strikewolf 1:dacf7db790f6 168
Strikewolf 1:dacf7db790f6 169 case STOP:
Strikewolf 1:dacf7db790f6 170 rightMotorPWM = 0;
Strikewolf 1:dacf7db790f6 171 leftMotorPWM = 0;
Strikewolf 1:dacf7db790f6 172 leftMotor1 = 0;
Strikewolf 1:dacf7db790f6 173 leftMotor2 = 1;
Strikewolf 1:dacf7db790f6 174 rightMotor1 = 1;
Strikewolf 1:dacf7db790f6 175 rightMotor2 = 0;
Strikewolf 1:dacf7db790f6 176 break;
Strikewolf 1:dacf7db790f6 177 }
Strikewolf 1:dacf7db790f6 178 }