Team Ascent / Mbed 2 deprecated TDPCode1

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@14:a243e6a78b2c, 2015-03-17 (annotated)

Committer:
Dbee16
Date:
Tue Mar 17 15:13:37 2015 +0000
Revision:
14:a243e6a78b2c
Parent:
13:c22e150048ae
Child:
16:be88d07a24fe
think PD_control is finished. not tested

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