Team Ascent / Mbed 2 deprecated TDPCode1

Dependencies:   mbed

Committer:
cmcmaster
Date:
Thu Mar 19 16:59:37 2015 +0000
Revision:
16:be88d07a24fe
Parent:
14:a243e6a78b2c
Child:
19:14b4b90bdf46
haaaaaaaaalllllllllllll

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cmcmaster 0:59d25bb7f825 1 #include "mbed.h"
cmcmaster 0:59d25bb7f825 2
cmcmaster 0:59d25bb7f825 3 //motor select pins
cmcmaster 0:59d25bb7f825 4 DigitalOut motor_lf(PTE2);
cmcmaster 0:59d25bb7f825 5 DigitalOut motor_lb(PTE3);
cmcmaster 0:59d25bb7f825 6 DigitalOut motor_rf(PTE4);
cmcmaster 0:59d25bb7f825 7 DigitalOut motor_rb(PTE5);
cmcmaster 0:59d25bb7f825 8
cmcmaster 0:59d25bb7f825 9 //Frequency of Pulse Width Modulated signal in Hz
cmcmaster 0:59d25bb7f825 10 #define PWM_FREQ 1000
cmcmaster 0:59d25bb7f825 11
cmcmaster 0:59d25bb7f825 12 //PWM pin (Enable 1 and 2)
cmcmaster 0:59d25bb7f825 13 PwmOut motor_l (PTC2);
cmcmaster 0:59d25bb7f825 14 PwmOut motor_r (PTE29);
cmcmaster 0:59d25bb7f825 15
cmcmaster 0:59d25bb7f825 16 //LED to test
Dbee16 4:6db8e0babea7 17 DigitalOut led(LED_RED);
Dbee16 1:2bab3a0bc3bc 18
Dbee16 8:eefbd3880d28 19 //Hall Effect Sensors
Dbee16 8:eefbd3880d28 20 InterruptIn hall_l (PTA1); //Hall Effect Sensor Output RobotLeft
Dbee16 8:eefbd3880d28 21 InterruptIn hall_r (PTA2); //Hall Effect Sensor Output RobotRight
Dbee16 8:eefbd3880d28 22 float time1_l, time2_l, time3_l, time1_r, time2_r, time3_r;
Dbee16 7:38def1917030 23 //Bluetooth Module to debug
Dbee16 7:38def1917030 24 Serial blue(PTC4,PTC3);
Dbee16 14:a243e6a78b2c 25 //********************************************************
Dbee16 14:a243e6a78b2c 26 float desired_duty_r; //USED FOR PD_CONTROL() SET IN set_dir()
Dbee16 14:a243e6a78b2c 27 float desired_duty_l;
Dbee16 7:38def1917030 28
cmcmaster 16:be88d07a24fe 29 float pres_duty_r, pres_duty_l;
cmcmaster 16:be88d07a24fe 30 float a_ratio;
cmcmaster 16:be88d07a24fe 31 float desired_ratio;
cmcmaster 16:be88d07a24fe 32 float perc_r;
cmcmaster 16:be88d07a24fe 33 float perc_l;
cmcmaster 16:be88d07a24fe 34 float perc_tot;
cmcmaster 16:be88d07a24fe 35 float threshold;
cmcmaster 16:be88d07a24fe 36 float limit;
cmcmaster 16:be88d07a24fe 37 float inc_value;
cmcmaster 16:be88d07a24fe 38
cmcmaster 16:be88d07a24fe 39 //HALL EFFECT INITIALISATIONS
cmcmaster 16:be88d07a24fe 40 Timer h_r, h_l;
cmcmaster 16:be88d07a24fe 41 float hall_rt1, hall_lt1, hall_rt2, hall_lt2 = 0; //hall right time & left time
cmcmaster 16:be88d07a24fe 42 float hall_rdt, hall_ldt;
cmcmaster 16:be88d07a24fe 43 int h_f_r, h_f_l; //hallflagright and left
cmcmaster 16:be88d07a24fe 44
cmcmaster 16:be88d07a24fe 45 void init_PD ()
cmcmaster 16:be88d07a24fe 46 {
cmcmaster 16:be88d07a24fe 47 //all these values have to be global; this function gets called each time there's a new direction/desired speed
cmcmaster 16:be88d07a24fe 48 a_ratio = hall_ldt/hall_rdt; //actual_ratio
cmcmaster 16:be88d07a24fe 49 desired_ratio = desired_duty_l/desired_duty_r; //desired_duty comes from set_direction()
cmcmaster 16:be88d07a24fe 50 perc_r = (1-((desired_duty_r-0.1)/desired_duty_r));//absolute vlaue of +-0.1 duty means a differnt percentage error for each duty value
cmcmaster 16:be88d07a24fe 51 perc_l = (1-((desired_duty_l-0.1)/desired_duty_l));
cmcmaster 16:be88d07a24fe 52 perc_tot = perc_r + perc_l;
cmcmaster 16:be88d07a24fe 53 threshold = desired_ratio*perc_tot; //dynamic threshold that will change according to duties
cmcmaster 16:be88d07a24fe 54 limit = 0.2*((desired_duty_l + desired_duty_r)/2); //twenty percent of the average duty
cmcmaster 16:be88d07a24fe 55 inc_value = 0.05*((desired_duty_l + desired_duty_r)/2); //probably need to change 5% from testing
cmcmaster 16:be88d07a24fe 56 }
Dbee16 12:2847b345b5cf 57 void set_direction( int direction, float duty_l, float duty_r)
cmcmaster 0:59d25bb7f825 58 {
Dbee16 13:c22e150048ae 59 desired_duty_r = duty_r; //USED FOR PD_CONTROL()
Dbee16 13:c22e150048ae 60 desired_duty_l = duty_l;
Dbee16 13:c22e150048ae 61
Dbee16 2:1feae3cb6731 62 switch( direction ) {
Dbee16 2:1feae3cb6731 63 case 0x11: { //forward
Dbee16 12:2847b345b5cf 64 motor_r.write( duty_r);
Dbee16 2:1feae3cb6731 65 //Look up ternary statements -- {condition ? value_if_true : value_if_false} This makes 0<result<1
Dbee16 12:2847b345b5cf 66 motor_l.write( duty_l);
Dbee16 2:1feae3cb6731 67
cmcmaster 0:59d25bb7f825 68 motor_rf=1;
cmcmaster 0:59d25bb7f825 69 motor_rb=0;
cmcmaster 0:59d25bb7f825 70 motor_lf=1;
cmcmaster 0:59d25bb7f825 71 motor_lb=0;
Dbee16 12:2847b345b5cf 72 blue.printf("Going forward, right:%f ; left:%f\n", duty_r, duty_l);
Dbee16 12:2847b345b5cf 73 break;
cmcmaster 0:59d25bb7f825 74 }
Dbee16 2:1feae3cb6731 75 case 0x00: { //backward
Dbee16 12:2847b345b5cf 76 motor_r.write( duty_r);
Dbee16 12:2847b345b5cf 77 motor_l.write( duty_l);
Dbee16 2:1feae3cb6731 78
cmcmaster 0:59d25bb7f825 79 motor_rf=0;
cmcmaster 0:59d25bb7f825 80 motor_rb=1;
cmcmaster 0:59d25bb7f825 81 motor_lf=0;
cmcmaster 0:59d25bb7f825 82 motor_lb=1;
Dbee16 12:2847b345b5cf 83 blue.printf("Going backward, right:%f ; left:%f\n", duty_r, duty_l);
Dbee16 12:2847b345b5cf 84 break;
cmcmaster 0:59d25bb7f825 85 }
Dbee16 2:1feae3cb6731 86 case 0x01: { //spin left -- Right forward, left backward
Dbee16 12:2847b345b5cf 87 motor_r.write( duty_r);
Dbee16 12:2847b345b5cf 88 motor_l.write( duty_l);
Dbee16 2:1feae3cb6731 89
Dbee16 2:1feae3cb6731 90 motor_rf=1;
Dbee16 2:1feae3cb6731 91 motor_rb=0;
Dbee16 2:1feae3cb6731 92 motor_lf=0;
Dbee16 2:1feae3cb6731 93 motor_lb=1;
Dbee16 12:2847b345b5cf 94 blue.printf("Spinning Left, right:%f ; left:%f\n", duty_r, duty_l);
Dbee16 12:2847b345b5cf 95 break;
Dbee16 2:1feae3cb6731 96 }
Dbee16 2:1feae3cb6731 97 case 0x10: { //spin right
Dbee16 12:2847b345b5cf 98 motor_r.write( duty_r);
Dbee16 12:2847b345b5cf 99 motor_l.write( duty_l);
Dbee16 2:1feae3cb6731 100
Dbee16 2:1feae3cb6731 101 motor_rf=0;
Dbee16 2:1feae3cb6731 102 motor_rb=1;
Dbee16 2:1feae3cb6731 103 motor_lf=1;
Dbee16 2:1feae3cb6731 104 motor_lb=0;
Dbee16 12:2847b345b5cf 105 blue.printf("Spinning Right, right:%f ; left:%f\n", duty_r, duty_l);
Dbee16 12:2847b345b5cf 106 break;
Dbee16 2:1feae3cb6731 107 }
cmcmaster 0:59d25bb7f825 108 }
Dbee16 14:a243e6a78b2c 109 init_PD ();
cmcmaster 0:59d25bb7f825 110 }
Dbee16 12:2847b345b5cf 111
cmcmaster 16:be88d07a24fe 112
cmcmaster 16:be88d07a24fe 113
Dbee16 8:eefbd3880d28 114 void hallr() //this just reading the various times.
Dbee16 8:eefbd3880d28 115 {
Dbee16 8:eefbd3880d28 116 if (h_f_r==2) {
Dbee16 12:2847b345b5cf 117 h_r.start();
Dbee16 12:2847b345b5cf 118 hall_rt1=0;
Dbee16 12:2847b345b5cf 119 hall_rt2 =0;
Dbee16 12:2847b345b5cf 120 blue.printf("Timer R Starts\n");
Dbee16 12:2847b345b5cf 121 h_f_r =1;
Dbee16 8:eefbd3880d28 122 } else if(h_f_r == 1) {
Dbee16 12:2847b345b5cf 123 hall_rt1 = h_r.read();
Dbee16 12:2847b345b5cf 124 //blue.printf("rt1 : %f\n", hall_rt1);
Dbee16 8:eefbd3880d28 125 h_f_r = 0;
Dbee16 12:2847b345b5cf 126 hall_rdt = hall_rt1-hall_rt2;
Dbee16 12:2847b345b5cf 127 } else if (h_f_r == 0) {
Dbee16 12:2847b345b5cf 128 hall_rt2 = h_r.read();
Dbee16 12:2847b345b5cf 129 //blue.printf("rt2 : %f\n", hall_rt2);
Dbee16 12:2847b345b5cf 130
Dbee16 8:eefbd3880d28 131 h_f_r = 1;
Dbee16 12:2847b345b5cf 132 hall_rdt = hall_rt2-hall_rt1;
Dbee16 8:eefbd3880d28 133 }
cmcmaster 16:be88d07a24fe 134 //blue.printf("Hall_R dt : %f\n", hall_rdt);
Dbee16 8:eefbd3880d28 135
Dbee16 8:eefbd3880d28 136 }
Dbee16 8:eefbd3880d28 137 void halll()
Dbee16 8:eefbd3880d28 138 {
Dbee16 12:2847b345b5cf 139 blue.printf("Left been triggered\n");
Dbee16 12:2847b345b5cf 140 if (h_f_l==2) {
Dbee16 12:2847b345b5cf 141 h_l.start();
Dbee16 12:2847b345b5cf 142 hall_lt1=0;
Dbee16 12:2847b345b5cf 143 hall_lt2 =0;
Dbee16 12:2847b345b5cf 144 blue.printf("Timer L starts\n");
Dbee16 12:2847b345b5cf 145 h_f_l = 1;
Dbee16 12:2847b345b5cf 146 } else if(h_f_l == 1) {
Dbee16 12:2847b345b5cf 147 hall_lt1 = h_l.read();
Dbee16 12:2847b345b5cf 148 //blue.printf("lt1 : %f\n", hall_lt1);
Dbee16 8:eefbd3880d28 149
Dbee16 12:2847b345b5cf 150 h_f_l = 0;
Dbee16 12:2847b345b5cf 151 hall_ldt = hall_lt1-hall_lt2;
Dbee16 12:2847b345b5cf 152 } else if (h_f_l == 0) {
Dbee16 12:2847b345b5cf 153 hall_lt2 = h_l.read();
Dbee16 12:2847b345b5cf 154 // blue.printf("lt1 : %f\n", hall_lt1);
Dbee16 12:2847b345b5cf 155
Dbee16 12:2847b345b5cf 156 h_f_l = 1;
Dbee16 12:2847b345b5cf 157 hall_ldt = hall_lt2-hall_lt1;
Dbee16 12:2847b345b5cf 158 }
Dbee16 13:c22e150048ae 159 blue.printf("Hall_L dt : %f\n", hall_ldt);
Dbee16 8:eefbd3880d28 160
Dbee16 8:eefbd3880d28 161 }
cmcmaster 16:be88d07a24fe 162
Dbee16 14:a243e6a78b2c 163
cmcmaster 16:be88d07a24fe 164
Dbee16 13:c22e150048ae 165 void PD_control ()
cmcmaster 0:59d25bb7f825 166 {
Dbee16 14:a243e6a78b2c 167
Dbee16 13:c22e150048ae 168 if(a_ratio < (desired_ratio - threshold)) { //if right is too big (bottom heavy denomotator)
Dbee16 14:a243e6a78b2c 169 if (pres_duty_l < (desired_duty_l + limit)) { //If you've not already set the duty too high. up_limit to be decided from testing
Dbee16 14:a243e6a78b2c 170 pres_duty_l =+ inc_value; //Make Left go faster. Incremental value requires testing.
Dbee16 14:a243e6a78b2c 171 motor_l.write( pres_duty_l);//actually changing the duty here
Dbee16 13:c22e150048ae 172 } else {
Dbee16 14:a243e6a78b2c 173 if (pres_duty_r > (desired_duty_r - limit)) { // if right isn't too small already
Dbee16 14:a243e6a78b2c 174 pres_duty_r =- inc_value; //make right go slower
Dbee16 14:a243e6a78b2c 175 motor_r.write(pres_duty_r);
Dbee16 13:c22e150048ae 176 }
Dbee16 13:c22e150048ae 177 //What to do when right is low limt AND left is up limit AND we've still not met ratio??
Dbee16 13:c22e150048ae 178 }
Dbee16 14:a243e6a78b2c 179 } else {
Dbee16 14:a243e6a78b2c 180 if(a_ratio > (desired_ratio + threshold)) {
Dbee16 14:a243e6a78b2c 181 if (pres_duty_r < (desired_duty_r + limit)) { //If you've not already set the duty too high. up_limit to be decided from testing
Dbee16 14:a243e6a78b2c 182 pres_duty_r =+ inc_value; //Make Left go faster. Incremental value requires testing.
Dbee16 14:a243e6a78b2c 183 motor_r.write( pres_duty_r);//actually changing the duty here
Dbee16 14:a243e6a78b2c 184 } else {
Dbee16 14:a243e6a78b2c 185 if (pres_duty_l > (desired_duty_l + limit)) { // if right isn't too small already
Dbee16 14:a243e6a78b2c 186 pres_duty_l =- inc_value; //make right go slower
Dbee16 14:a243e6a78b2c 187 motor_l.write(pres_duty_l);
Dbee16 13:c22e150048ae 188 }
Dbee16 14:a243e6a78b2c 189 //What to do when left is low limt AND right is up limit AND we've still not met ratio??
Dbee16 13:c22e150048ae 190 }
Dbee16 14:a243e6a78b2c 191 }
Dbee16 8:eefbd3880d28 192
Dbee16 14:a243e6a78b2c 193 }
Dbee16 13:c22e150048ae 194
Dbee16 13:c22e150048ae 195
Dbee16 13:c22e150048ae 196
Dbee16 14:a243e6a78b2c 197 }
Dbee16 14:a243e6a78b2c 198 int main()
Dbee16 14:a243e6a78b2c 199 {
Dbee16 14:a243e6a78b2c 200 h_f_r = 2;
Dbee16 14:a243e6a78b2c 201 h_f_l = 2;
Dbee16 14:a243e6a78b2c 202 //attaching hall_r and hall_r so that it calls hall() on a rising and falling edge
Dbee16 14:a243e6a78b2c 203 hall_r.rise(&hallr);
Dbee16 13:c22e150048ae 204
Dbee16 14:a243e6a78b2c 205 hall_l.rise(&halll);
Dbee16 2:1feae3cb6731 206 //Set PWM frequency to 1000Hz
Dbee16 8:eefbd3880d28 207
Dbee16 8:eefbd3880d28 208
Dbee16 14:a243e6a78b2c 209 motor_l.period( 1.0f / (float) PWM_FREQ);
Dbee16 14:a243e6a78b2c 210 motor_r.period( 1.0f / (float) PWM_FREQ);
cmcmaster 0:59d25bb7f825 211 //Initialise direction to nothing.
Dbee16 14:a243e6a78b2c 212 motor_rf=0;
Dbee16 14:a243e6a78b2c 213 motor_rb=0;
Dbee16 14:a243e6a78b2c 214 motor_lf=0;
Dbee16 14:a243e6a78b2c 215 motor_lb=0;
Dbee16 14:a243e6a78b2c 216 wait(1);
cmcmaster 16:be88d07a24fe 217 set_direction(0x11, 0.5,0.5);
Dbee16 14:a243e6a78b2c 218 while(1) {
Dbee16 12:2847b345b5cf 219 }
Dbee16 14:a243e6a78b2c 220 }