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@19:14b4b90bdf46, 2015-03-23 (annotated)

Committer:
Dbee16
Date:
Mon Mar 23 16:06:15 2015 +0000
Revision:
19:14b4b90bdf46
Parent:
16:be88d07a24fe 
changed initiliastations

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