Ryan Vasquez / Mbed 2 deprecated GearboxFW

This is firmware for the smart gearbox! Features: Forward Reverse

Dependencies:   mbed

main.cpp@2:fe723d003d1f, 2015-10-29 (annotated)

Committer:
rvasquez6089
Date:
Thu Oct 29 20:56:55 2015 +0000
Revision:
2:fe723d003d1f
Parent:
1:12e11042b2e9 
Forward and reverse Implemented

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rvasquez6089 0:39bb015c8ac2 1 #include "mbed.h"
rvasquez6089 0:39bb015c8ac2 2 PwmOut MOT_C(PA_9);
rvasquez6089 0:39bb015c8ac2 3 DigitalOut MOT_B(PA_8);
rvasquez6089 0:39bb015c8ac2 4 PwmOut MOT_D(PA_10);
rvasquez6089 0:39bb015c8ac2 5 DigitalOut MOT_A(PA_7);
rvasquez6089 2:fe723d003d1f 6 InterruptIn Button_P(PC_13);
rvasquez6089 2:fe723d003d1f 7 InterruptIn Button_D(PC_14);
rvasquez6089 0:39bb015c8ac2 8 DigitalOut Orange(PB_0);
rvasquez6089 0:39bb015c8ac2 9 DigitalOut myled(LED1);
rvasquez6089 0:39bb015c8ac2 10 Ticker Motor;
rvasquez6089 0:39bb015c8ac2 11 int go_to_sleep = 0;
rvasquez6089 0:39bb015c8ac2 12 DigitalOut CSENSE_EN(PC_9);
rvasquez6089 0:39bb015c8ac2 13 AnalogIn CSENSE(PC_0);
rvasquez6089 0:39bb015c8ac2 14 AnalogIn VBAT(PC_1);
rvasquez6089 0:39bb015c8ac2 15 DigitalOut SEL1(PA_11);
rvasquez6089 0:39bb015c8ac2 16 DigitalOut SEL2(PA_12);
rvasquez6089 0:39bb015c8ac2 17 void run_300();
rvasquez6089 0:39bb015c8ac2 18 Timeout go_sleep;
rvasquez6089 2:fe723d003d1f 19 bool F = 0;
rvasquez6089 2:fe723d003d1f 20 bool R = 0;
rvasquez6089 0:39bb015c8ac2 21
rvasquez6089 0:39bb015c8ac2 22
rvasquez6089 0:39bb015c8ac2 23 bool battery_status()
rvasquez6089 0:39bb015c8ac2 24 {
rvasquez6089 0:39bb015c8ac2 25 double battery_level = 0.0;
rvasquez6089 0:39bb015c8ac2 26 const int on = 50;
rvasquez6089 0:39bb015c8ac2 27 const int off = 100;
rvasquez6089 0:39bb015c8ac2 28 bool low_bat = 0;
rvasquez6089 0:39bb015c8ac2 29 for(int i = 0; i < 1000; i++)
rvasquez6089 0:39bb015c8ac2 30 {
rvasquez6089 0:39bb015c8ac2 31 battery_level = battery_level + VBAT.read();
rvasquez6089 0:39bb015c8ac2 32 }
rvasquez6089 0:39bb015c8ac2 33 battery_level = battery_level/1000.0;
rvasquez6089 0:39bb015c8ac2 34 battery_level = battery_level*3.3*2.0;
rvasquez6089 0:39bb015c8ac2 35 if(battery_level >= 3.825)
rvasquez6089 0:39bb015c8ac2 36 {
rvasquez6089 0:39bb015c8ac2 37 wait_ms(250);
rvasquez6089 0:39bb015c8ac2 38 Orange = 1;
rvasquez6089 0:39bb015c8ac2 39 wait_ms(on);
rvasquez6089 0:39bb015c8ac2 40 Orange = 0;
rvasquez6089 0:39bb015c8ac2 41 wait_ms(off);
rvasquez6089 0:39bb015c8ac2 42 Orange = 1;
rvasquez6089 0:39bb015c8ac2 43 wait_ms(on);
rvasquez6089 0:39bb015c8ac2 44 Orange = 0;
rvasquez6089 0:39bb015c8ac2 45 wait_ms(off);
rvasquez6089 0:39bb015c8ac2 46 Orange = 1;
rvasquez6089 0:39bb015c8ac2 47 wait_ms(on);
rvasquez6089 0:39bb015c8ac2 48 Orange = 0;
rvasquez6089 0:39bb015c8ac2 49 wait_ms(off);
rvasquez6089 0:39bb015c8ac2 50 Orange = 1;
rvasquez6089 0:39bb015c8ac2 51 wait_ms(on);
rvasquez6089 0:39bb015c8ac2 52 Orange = 0;
rvasquez6089 0:39bb015c8ac2 53 wait_ms(off);
rvasquez6089 0:39bb015c8ac2 54 low_bat = 0;
rvasquez6089 0:39bb015c8ac2 55 }
rvasquez6089 0:39bb015c8ac2 56 if(battery_level >= 3.45 && battery_level < 3.825)
rvasquez6089 0:39bb015c8ac2 57 {
rvasquez6089 0:39bb015c8ac2 58 wait_ms(250);
rvasquez6089 0:39bb015c8ac2 59 Orange = 1;
rvasquez6089 0:39bb015c8ac2 60 wait_ms(on);
rvasquez6089 0:39bb015c8ac2 61 Orange = 0;
rvasquez6089 0:39bb015c8ac2 62 wait_ms(off);
rvasquez6089 0:39bb015c8ac2 63 Orange = 1;
rvasquez6089 0:39bb015c8ac2 64 wait_ms(on);
rvasquez6089 0:39bb015c8ac2 65 Orange = 0;
rvasquez6089 0:39bb015c8ac2 66 wait_ms(off);
rvasquez6089 0:39bb015c8ac2 67 Orange = 1;
rvasquez6089 0:39bb015c8ac2 68 wait_ms(on);
rvasquez6089 0:39bb015c8ac2 69 Orange = 0;
rvasquez6089 0:39bb015c8ac2 70 wait_ms(off);
rvasquez6089 0:39bb015c8ac2 71 low_bat = 0;
rvasquez6089 0:39bb015c8ac2 72 }
rvasquez6089 0:39bb015c8ac2 73 if(battery_level >= 3.075 && battery_level < 3.45)
rvasquez6089 0:39bb015c8ac2 74 {
rvasquez6089 0:39bb015c8ac2 75 wait_ms(250);
rvasquez6089 0:39bb015c8ac2 76 Orange = 1;
rvasquez6089 0:39bb015c8ac2 77 wait_ms(on);
rvasquez6089 0:39bb015c8ac2 78 Orange = 0;
rvasquez6089 0:39bb015c8ac2 79 wait_ms(off);
rvasquez6089 0:39bb015c8ac2 80 Orange = 1;
rvasquez6089 0:39bb015c8ac2 81 wait_ms(on);
rvasquez6089 0:39bb015c8ac2 82 Orange = 0;
rvasquez6089 0:39bb015c8ac2 83 wait_ms(off);
rvasquez6089 0:39bb015c8ac2 84 low_bat = 0;
rvasquez6089 0:39bb015c8ac2 85 }
rvasquez6089 0:39bb015c8ac2 86 if(battery_level < 3.075 && battery_level >= 2.9)
rvasquez6089 0:39bb015c8ac2 87 {
rvasquez6089 0:39bb015c8ac2 88 wait_ms(250);
rvasquez6089 0:39bb015c8ac2 89 Orange = 1;
rvasquez6089 0:39bb015c8ac2 90 wait_ms(on);
rvasquez6089 0:39bb015c8ac2 91 Orange = 0;
rvasquez6089 0:39bb015c8ac2 92 wait_ms(off);
rvasquez6089 0:39bb015c8ac2 93 low_bat = 0;
rvasquez6089 0:39bb015c8ac2 94 }
rvasquez6089 0:39bb015c8ac2 95 if(battery_level < 2.9)
rvasquez6089 0:39bb015c8ac2 96 {
rvasquez6089 0:39bb015c8ac2 97 wait_ms(250);
rvasquez6089 0:39bb015c8ac2 98 Orange = 1;
rvasquez6089 0:39bb015c8ac2 99 wait_ms(75);
rvasquez6089 0:39bb015c8ac2 100 Orange = 0;
rvasquez6089 0:39bb015c8ac2 101 wait_ms(150);
rvasquez6089 0:39bb015c8ac2 102 low_bat = 1;
rvasquez6089 0:39bb015c8ac2 103 }
rvasquez6089 0:39bb015c8ac2 104 return low_bat;
rvasquez6089 0:39bb015c8ac2 105 }
rvasquez6089 0:39bb015c8ac2 106
rvasquez6089 2:fe723d003d1f 107 void pressed_F()
rvasquez6089 0:39bb015c8ac2 108 {
rvasquez6089 0:39bb015c8ac2 109 printf("Button pressed\n");
rvasquez6089 0:39bb015c8ac2 110 //battery_status();
rvasquez6089 0:39bb015c8ac2 111 go_to_sleep = !go_to_sleep;
rvasquez6089 0:39bb015c8ac2 112 if(go_to_sleep == 1)
rvasquez6089 0:39bb015c8ac2 113 {
rvasquez6089 0:39bb015c8ac2 114 //battery_status();
rvasquez6089 2:fe723d003d1f 115 //F = 0;
rvasquez6089 0:39bb015c8ac2 116 go_sleep.detach();
rvasquez6089 0:39bb015c8ac2 117 //Orange = 0.0f;
rvasquez6089 0:39bb015c8ac2 118 }
rvasquez6089 0:39bb015c8ac2 119 if(go_to_sleep == 0)
rvasquez6089 0:39bb015c8ac2 120 {
rvasquez6089 0:39bb015c8ac2 121 if(!(battery_status()))
rvasquez6089 0:39bb015c8ac2 122 {
rvasquez6089 2:fe723d003d1f 123 go_sleep.attach(&pressed_F, 10.0);
rvasquez6089 2:fe723d003d1f 124 F = 1;
rvasquez6089 2:fe723d003d1f 125
rvasquez6089 2:fe723d003d1f 126 }
rvasquez6089 2:fe723d003d1f 127 }
rvasquez6089 2:fe723d003d1f 128 }
rvasquez6089 2:fe723d003d1f 129
rvasquez6089 2:fe723d003d1f 130 void pressed_R()
rvasquez6089 2:fe723d003d1f 131 {
rvasquez6089 2:fe723d003d1f 132 printf("Button pressed\n");
rvasquez6089 2:fe723d003d1f 133 //battery_status();
rvasquez6089 2:fe723d003d1f 134 go_to_sleep = !go_to_sleep;
rvasquez6089 2:fe723d003d1f 135 if(go_to_sleep == 1)
rvasquez6089 2:fe723d003d1f 136 {
rvasquez6089 2:fe723d003d1f 137 //battery_status();
rvasquez6089 2:fe723d003d1f 138 //R = 0;
rvasquez6089 2:fe723d003d1f 139 go_sleep.detach();
rvasquez6089 2:fe723d003d1f 140 //Orange = 0.0f;
rvasquez6089 2:fe723d003d1f 141 }
rvasquez6089 2:fe723d003d1f 142 if(go_to_sleep == 0)
rvasquez6089 2:fe723d003d1f 143 {
rvasquez6089 2:fe723d003d1f 144 if(!(battery_status()))
rvasquez6089 2:fe723d003d1f 145 {
rvasquez6089 2:fe723d003d1f 146 go_sleep.attach(&pressed_R, 10.0);
rvasquez6089 2:fe723d003d1f 147 R = 1;
rvasquez6089 2:fe723d003d1f 148
rvasquez6089 0:39bb015c8ac2 149 }
rvasquez6089 0:39bb015c8ac2 150 }
rvasquez6089 0:39bb015c8ac2 151 }
rvasquez6089 0:39bb015c8ac2 152
rvasquez6089 0:39bb015c8ac2 153 void run_300_F()
rvasquez6089 0:39bb015c8ac2 154 {
rvasquez6089 0:39bb015c8ac2 155
rvasquez6089 0:39bb015c8ac2 156 float current;
rvasquez6089 0:39bb015c8ac2 157 int output;
rvasquez6089 0:39bb015c8ac2 158 MOT_D = 0.0;
rvasquez6089 0:39bb015c8ac2 159 MOT_A = 0;
rvasquez6089 0:39bb015c8ac2 160 MOT_B = 1;
rvasquez6089 0:39bb015c8ac2 161 current = 0.0;
rvasquez6089 0:39bb015c8ac2 162 for(int i = 0; i < 10; i++)
rvasquez6089 0:39bb015c8ac2 163 {
rvasquez6089 0:39bb015c8ac2 164 current = current + CSENSE.read();
rvasquez6089 0:39bb015c8ac2 165 //wait_us(10);
rvasquez6089 0:39bb015c8ac2 166 }
rvasquez6089 0:39bb015c8ac2 167 current = current/10;
rvasquez6089 0:39bb015c8ac2 168 current = ((current*3.3f)/100.0f)/(0.015f);
rvasquez6089 0:39bb015c8ac2 169 current = current*1000.0f;
rvasquez6089 0:39bb015c8ac2 170 output = static_cast<int>(current);
rvasquez6089 0:39bb015c8ac2 171 //pc.printf("Motor Current = %d mA\r\n",output );
rvasquez6089 0:39bb015c8ac2 172
rvasquez6089 0:39bb015c8ac2 173 //wait(0.001);
rvasquez6089 0:39bb015c8ac2 174 if(output >= 300)
rvasquez6089 0:39bb015c8ac2 175 {
rvasquez6089 0:39bb015c8ac2 176 MOT_C = MOT_C - (0.01f*(((output-300)/1)));
rvasquez6089 0:39bb015c8ac2 177 }
rvasquez6089 0:39bb015c8ac2 178 if(output < 300)
rvasquez6089 0:39bb015c8ac2 179 {
rvasquez6089 0:39bb015c8ac2 180 MOT_C = 0.9f;
rvasquez6089 0:39bb015c8ac2 181 }
rvasquez6089 0:39bb015c8ac2 182 }
rvasquez6089 0:39bb015c8ac2 183
rvasquez6089 0:39bb015c8ac2 184 void run_1200_F()
rvasquez6089 0:39bb015c8ac2 185 {
rvasquez6089 0:39bb015c8ac2 186
rvasquez6089 0:39bb015c8ac2 187 float current;
rvasquez6089 0:39bb015c8ac2 188 int output;
rvasquez6089 0:39bb015c8ac2 189
rvasquez6089 0:39bb015c8ac2 190
rvasquez6089 0:39bb015c8ac2 191 MOT_D = 0.0;
rvasquez6089 0:39bb015c8ac2 192 MOT_A = 0;
rvasquez6089 0:39bb015c8ac2 193 MOT_B = 1;
rvasquez6089 0:39bb015c8ac2 194 current = 0.0;
rvasquez6089 0:39bb015c8ac2 195 for(int i = 0; i < 10; i++)
rvasquez6089 0:39bb015c8ac2 196 {
rvasquez6089 0:39bb015c8ac2 197 current = current + CSENSE.read();
rvasquez6089 0:39bb015c8ac2 198 //wait_us(10);
rvasquez6089 0:39bb015c8ac2 199 }
rvasquez6089 0:39bb015c8ac2 200 current = current/10;
rvasquez6089 0:39bb015c8ac2 201 current = ((current*3.3f)/100.0f)/(0.015f);
rvasquez6089 0:39bb015c8ac2 202 current = current*1000.0f;
rvasquez6089 0:39bb015c8ac2 203 output = static_cast<int>(current);
rvasquez6089 0:39bb015c8ac2 204 //pc.printf("Motor Current = %d mA\r\n",output );
rvasquez6089 0:39bb015c8ac2 205
rvasquez6089 0:39bb015c8ac2 206 //wait(0.001);
rvasquez6089 0:39bb015c8ac2 207 if(output >= 1200)
rvasquez6089 0:39bb015c8ac2 208 {
rvasquez6089 0:39bb015c8ac2 209 MOT_C = MOT_C - (0.01f*(((output-1200)/1)));
rvasquez6089 0:39bb015c8ac2 210 }
rvasquez6089 0:39bb015c8ac2 211 if(output < 1200)
rvasquez6089 0:39bb015c8ac2 212 {
rvasquez6089 0:39bb015c8ac2 213 MOT_C = 1.0f;
rvasquez6089 0:39bb015c8ac2 214 }
rvasquez6089 0:39bb015c8ac2 215 }
rvasquez6089 0:39bb015c8ac2 216
rvasquez6089 0:39bb015c8ac2 217 void run_1500_F()
rvasquez6089 0:39bb015c8ac2 218 {
rvasquez6089 0:39bb015c8ac2 219
rvasquez6089 0:39bb015c8ac2 220 float current;
rvasquez6089 0:39bb015c8ac2 221 int output;
rvasquez6089 0:39bb015c8ac2 222
rvasquez6089 0:39bb015c8ac2 223
rvasquez6089 0:39bb015c8ac2 224 MOT_D = 0.0;
rvasquez6089 0:39bb015c8ac2 225 MOT_A = 0;
rvasquez6089 0:39bb015c8ac2 226 MOT_B = 1;
rvasquez6089 0:39bb015c8ac2 227 current = 0.0;
rvasquez6089 0:39bb015c8ac2 228 for(int i = 0; i < 10; i++)
rvasquez6089 0:39bb015c8ac2 229 {
rvasquez6089 0:39bb015c8ac2 230 current = current + CSENSE.read();
rvasquez6089 0:39bb015c8ac2 231 //wait_us(10);
rvasquez6089 0:39bb015c8ac2 232 }
rvasquez6089 0:39bb015c8ac2 233 current = current/10;
rvasquez6089 0:39bb015c8ac2 234 current = ((current*3.3f)/100.0f)/(0.015f);
rvasquez6089 0:39bb015c8ac2 235 current = current*1000.0f;
rvasquez6089 0:39bb015c8ac2 236 output = static_cast<int>(current);
rvasquez6089 0:39bb015c8ac2 237 //pc.printf("Motor Current = %d mA\r\n",output );
rvasquez6089 0:39bb015c8ac2 238
rvasquez6089 0:39bb015c8ac2 239 //wait(0.001);
rvasquez6089 0:39bb015c8ac2 240 if(output >= 1500)
rvasquez6089 0:39bb015c8ac2 241 {
rvasquez6089 0:39bb015c8ac2 242 MOT_C = MOT_C - (0.01f*(((output-1500)/1)));
rvasquez6089 0:39bb015c8ac2 243 }
rvasquez6089 0:39bb015c8ac2 244 if(output < 1500)
rvasquez6089 0:39bb015c8ac2 245 {
rvasquez6089 0:39bb015c8ac2 246 MOT_C = 1.0f;
rvasquez6089 0:39bb015c8ac2 247 }
rvasquez6089 0:39bb015c8ac2 248 }
rvasquez6089 0:39bb015c8ac2 249
rvasquez6089 0:39bb015c8ac2 250
rvasquez6089 0:39bb015c8ac2 251 void run_300_R()
rvasquez6089 0:39bb015c8ac2 252 {
rvasquez6089 0:39bb015c8ac2 253
rvasquez6089 0:39bb015c8ac2 254 float current;
rvasquez6089 0:39bb015c8ac2 255 int output;
rvasquez6089 0:39bb015c8ac2 256 MOT_B = 0.0;
rvasquez6089 0:39bb015c8ac2 257 MOT_C = 0;
rvasquez6089 0:39bb015c8ac2 258 MOT_A = 1;
rvasquez6089 0:39bb015c8ac2 259 current = 0.0;
rvasquez6089 0:39bb015c8ac2 260 for(int i = 0; i < 10; i++)
rvasquez6089 0:39bb015c8ac2 261 {
rvasquez6089 0:39bb015c8ac2 262 current = current + CSENSE.read();
rvasquez6089 0:39bb015c8ac2 263 //wait_us(10);
rvasquez6089 0:39bb015c8ac2 264 }
rvasquez6089 0:39bb015c8ac2 265 current = current/10;
rvasquez6089 0:39bb015c8ac2 266 current = ((current*3.3f)/100.0f)/(0.015f);
rvasquez6089 0:39bb015c8ac2 267 current = current*1000.0f;
rvasquez6089 0:39bb015c8ac2 268 output = static_cast<int>(current);
rvasquez6089 0:39bb015c8ac2 269 //pc.printf("Motor Current = %d mA\r\n",output );
rvasquez6089 0:39bb015c8ac2 270
rvasquez6089 0:39bb015c8ac2 271 //wait(0.001);
rvasquez6089 0:39bb015c8ac2 272 if(output >= 300)
rvasquez6089 0:39bb015c8ac2 273 {
rvasquez6089 0:39bb015c8ac2 274 MOT_D = MOT_D - (0.01f*(((output-300)/1)));
rvasquez6089 0:39bb015c8ac2 275 }
rvasquez6089 0:39bb015c8ac2 276 if(output < 300)
rvasquez6089 0:39bb015c8ac2 277 {
rvasquez6089 0:39bb015c8ac2 278 MOT_D = 0.9f;
rvasquez6089 0:39bb015c8ac2 279 }
rvasquez6089 0:39bb015c8ac2 280 }
rvasquez6089 0:39bb015c8ac2 281
rvasquez6089 0:39bb015c8ac2 282 void run_1500_R()
rvasquez6089 0:39bb015c8ac2 283 {
rvasquez6089 0:39bb015c8ac2 284
rvasquez6089 0:39bb015c8ac2 285 float current;
rvasquez6089 0:39bb015c8ac2 286 int output;
rvasquez6089 0:39bb015c8ac2 287 MOT_B = 0.0;
rvasquez6089 0:39bb015c8ac2 288 MOT_C = 0;
rvasquez6089 0:39bb015c8ac2 289 MOT_A = 1;
rvasquez6089 0:39bb015c8ac2 290 current = 0.0;
rvasquez6089 0:39bb015c8ac2 291 for(int i = 0; i < 10; i++)
rvasquez6089 0:39bb015c8ac2 292 {
rvasquez6089 0:39bb015c8ac2 293 current = current + CSENSE.read();
rvasquez6089 0:39bb015c8ac2 294 //wait_us(10);
rvasquez6089 0:39bb015c8ac2 295 }
rvasquez6089 0:39bb015c8ac2 296 current = current/10;
rvasquez6089 0:39bb015c8ac2 297 current = ((current*3.3f)/100.0f)/(0.015f);
rvasquez6089 0:39bb015c8ac2 298 current = current*1000.0f;
rvasquez6089 0:39bb015c8ac2 299 output = static_cast<int>(current);
rvasquez6089 0:39bb015c8ac2 300 //pc.printf("Motor Current = %d mA\r\n",output );
rvasquez6089 0:39bb015c8ac2 301
rvasquez6089 0:39bb015c8ac2 302 //wait(0.001);
rvasquez6089 0:39bb015c8ac2 303 if(output >= 1500)
rvasquez6089 0:39bb015c8ac2 304 {
rvasquez6089 0:39bb015c8ac2 305 MOT_D = MOT_D - (0.01f*(((output-1500)/1)));
rvasquez6089 0:39bb015c8ac2 306 }
rvasquez6089 0:39bb015c8ac2 307 if(output < 1500)
rvasquez6089 0:39bb015c8ac2 308 {
rvasquez6089 0:39bb015c8ac2 309 MOT_D = 1.0f;
rvasquez6089 0:39bb015c8ac2 310 }
rvasquez6089 0:39bb015c8ac2 311 }
rvasquez6089 0:39bb015c8ac2 312
rvasquez6089 1:12e11042b2e9 313 void run_4500_R()
rvasquez6089 1:12e11042b2e9 314 {
rvasquez6089 1:12e11042b2e9 315
rvasquez6089 1:12e11042b2e9 316 float current;
rvasquez6089 1:12e11042b2e9 317 int output;
rvasquez6089 1:12e11042b2e9 318 MOT_B = 0.0;
rvasquez6089 1:12e11042b2e9 319 MOT_C = 0;
rvasquez6089 1:12e11042b2e9 320 MOT_A = 1;
rvasquez6089 1:12e11042b2e9 321 current = 0.0;
rvasquez6089 1:12e11042b2e9 322 for(int i = 0; i < 10; i++)
rvasquez6089 1:12e11042b2e9 323 {
rvasquez6089 1:12e11042b2e9 324 current = current + CSENSE.read();
rvasquez6089 1:12e11042b2e9 325 //wait_us(10);
rvasquez6089 1:12e11042b2e9 326 }
rvasquez6089 1:12e11042b2e9 327 current = current/10;
rvasquez6089 1:12e11042b2e9 328 current = ((current*3.3f)/25.0f)/(0.015f);
rvasquez6089 1:12e11042b2e9 329 current = current*1000.0f;
rvasquez6089 1:12e11042b2e9 330 output = static_cast<int>(current);
rvasquez6089 1:12e11042b2e9 331 //pc.printf("Motor Current = %d mA\r\n",output );
rvasquez6089 1:12e11042b2e9 332
rvasquez6089 1:12e11042b2e9 333 //wait(0.001);
rvasquez6089 1:12e11042b2e9 334 if(output >= 4500)
rvasquez6089 1:12e11042b2e9 335 {
rvasquez6089 1:12e11042b2e9 336 MOT_D = MOT_D - (0.01f*(((output-4500)/1)));
rvasquez6089 1:12e11042b2e9 337 }
rvasquez6089 1:12e11042b2e9 338 if(output < 4500)
rvasquez6089 1:12e11042b2e9 339 {
rvasquez6089 1:12e11042b2e9 340 MOT_D = 1.0f;
rvasquez6089 1:12e11042b2e9 341 }
rvasquez6089 1:12e11042b2e9 342 }
rvasquez6089 1:12e11042b2e9 343
rvasquez6089 2:fe723d003d1f 344 void run_4500_F()
rvasquez6089 2:fe723d003d1f 345 {
rvasquez6089 1:12e11042b2e9 346
rvasquez6089 2:fe723d003d1f 347 float current;
rvasquez6089 2:fe723d003d1f 348 int output;
rvasquez6089 2:fe723d003d1f 349
rvasquez6089 2:fe723d003d1f 350
rvasquez6089 2:fe723d003d1f 351 MOT_D = 0.0;
rvasquez6089 2:fe723d003d1f 352 MOT_A = 0;
rvasquez6089 2:fe723d003d1f 353 MOT_B = 1;
rvasquez6089 2:fe723d003d1f 354 current = 0.0;
rvasquez6089 2:fe723d003d1f 355 for(int i = 0; i < 10; i++)
rvasquez6089 2:fe723d003d1f 356 {
rvasquez6089 2:fe723d003d1f 357 current = current + CSENSE.read();
rvasquez6089 2:fe723d003d1f 358 //wait_us(10);
rvasquez6089 2:fe723d003d1f 359 }
rvasquez6089 2:fe723d003d1f 360 current = current/10;
rvasquez6089 2:fe723d003d1f 361 current = ((current*3.3f)/100.0f)/(0.015f);
rvasquez6089 2:fe723d003d1f 362 current = current*1000.0f;
rvasquez6089 2:fe723d003d1f 363 output = static_cast<int>(current);
rvasquez6089 2:fe723d003d1f 364 //pc.printf("Motor Current = %d mA\r\n",output );
rvasquez6089 2:fe723d003d1f 365
rvasquez6089 2:fe723d003d1f 366 //wait(0.001);
rvasquez6089 2:fe723d003d1f 367 if(output >= 4500)
rvasquez6089 2:fe723d003d1f 368 {
rvasquez6089 2:fe723d003d1f 369 MOT_C = MOT_C - (0.01f*(((output-4500)/1)));
rvasquez6089 2:fe723d003d1f 370 }
rvasquez6089 2:fe723d003d1f 371 if(output < 4500)
rvasquez6089 2:fe723d003d1f 372 {
rvasquez6089 2:fe723d003d1f 373 MOT_C = 1.0f;
rvasquez6089 2:fe723d003d1f 374 }
rvasquez6089 2:fe723d003d1f 375 }
rvasquez6089 0:39bb015c8ac2 376
rvasquez6089 0:39bb015c8ac2 377 int main()
rvasquez6089 0:39bb015c8ac2 378 {
rvasquez6089 0:39bb015c8ac2 379 int i = 0;
rvasquez6089 0:39bb015c8ac2 380 MOT_A = 0;
rvasquez6089 0:39bb015c8ac2 381 MOT_B = 0;
rvasquez6089 0:39bb015c8ac2 382 MOT_C = 0.0f;
rvasquez6089 0:39bb015c8ac2 383 MOT_D = 0.0f;
rvasquez6089 0:39bb015c8ac2 384 //Orange = 0.0f;
rvasquez6089 2:fe723d003d1f 385 Button_P.fall(&pressed_F);
rvasquez6089 2:fe723d003d1f 386 Button_D.fall(&pressed_R);
rvasquez6089 0:39bb015c8ac2 387 battery_status();
rvasquez6089 0:39bb015c8ac2 388 MOT_C.period(0.00005);
rvasquez6089 0:39bb015c8ac2 389 MOT_D.period(0.00005);
rvasquez6089 0:39bb015c8ac2 390 //Orange.period(0.005);
rvasquez6089 0:39bb015c8ac2 391 go_to_sleep = 1;
rvasquez6089 0:39bb015c8ac2 392 while (1) {
rvasquez6089 0:39bb015c8ac2 393 if (go_to_sleep) {
rvasquez6089 0:39bb015c8ac2 394
rvasquez6089 2:fe723d003d1f 395 F = 0;
rvasquez6089 2:fe723d003d1f 396 R = 0;
rvasquez6089 0:39bb015c8ac2 397 myled = 0;
rvasquez6089 0:39bb015c8ac2 398 //printf("%d: Entering sleep (press user button to resume)\n", i);
rvasquez6089 0:39bb015c8ac2 399 Motor.detach();
rvasquez6089 0:39bb015c8ac2 400 SEL1 = 0;
rvasquez6089 0:39bb015c8ac2 401 SEL2 = 0;
rvasquez6089 0:39bb015c8ac2 402 CSENSE_EN = 0;
rvasquez6089 0:39bb015c8ac2 403 Orange = 0;
rvasquez6089 0:39bb015c8ac2 404 MOT_D = 0.0f;
rvasquez6089 0:39bb015c8ac2 405 MOT_A = 0;
rvasquez6089 0:39bb015c8ac2 406 MOT_C = 0.0f;
rvasquez6089 0:39bb015c8ac2 407 MOT_B = 0;
rvasquez6089 0:39bb015c8ac2 408 //Orange = 0.0f;
rvasquez6089 0:39bb015c8ac2 409 sleep();
rvasquez6089 0:39bb015c8ac2 410 //deepsleep();
rvasquez6089 0:39bb015c8ac2 411 wait(0.1);
rvasquez6089 1:12e11042b2e9 412 SEL1=0;
rvasquez6089 0:39bb015c8ac2 413 SEL2=1;
rvasquez6089 0:39bb015c8ac2 414 CSENSE_EN = 1;
rvasquez6089 0:39bb015c8ac2 415 } else {
rvasquez6089 0:39bb015c8ac2 416 //printf("%d: Running\n", i);
rvasquez6089 0:39bb015c8ac2 417 myled = 1;
rvasquez6089 0:39bb015c8ac2 418 Orange = 1;
rvasquez6089 2:fe723d003d1f 419 if(R && !F)
rvasquez6089 2:fe723d003d1f 420 {
rvasquez6089 2:fe723d003d1f 421 run_4500_R();
rvasquez6089 2:fe723d003d1f 422 }
rvasquez6089 2:fe723d003d1f 423 if(F && !R)
rvasquez6089 2:fe723d003d1f 424 {
rvasquez6089 2:fe723d003d1f 425 run_4500_F();
rvasquez6089 2:fe723d003d1f 426 }
rvasquez6089 0:39bb015c8ac2 427 }
rvasquez6089 0:39bb015c8ac2 428 i++;
rvasquez6089 0:39bb015c8ac2 429 }
rvasquez6089 0:39bb015c8ac2 430 }