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@16:be88d07a24fe, 2015-03-19 (annotated)
- 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?
User | Revision | Line number | New 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 | } |