Ryan Vasquez
/
GearboxFW
This is firmware for the smart gearbox! Features: Forward Reverse
main.cpp@0:39bb015c8ac2, 2015-10-29 (annotated)
- Committer:
- rvasquez6089
- Date:
- Thu Oct 29 04:25:23 2015 +0000
- Revision:
- 0:39bb015c8ac2
- Child:
- 1:12e11042b2e9
REV 1.; Indicates battery life with flashes, spins in counter clockwise direction with 1.5A current limit.
Who changed what in which revision?
User | Revision | Line number | New 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 | 0:39bb015c8ac2 | 6 | InterruptIn event(USER_BUTTON); |
rvasquez6089 | 0:39bb015c8ac2 | 7 | DigitalOut Orange(PB_0); |
rvasquez6089 | 0:39bb015c8ac2 | 8 | DigitalOut myled(LED1); |
rvasquez6089 | 0:39bb015c8ac2 | 9 | Ticker Motor; |
rvasquez6089 | 0:39bb015c8ac2 | 10 | int go_to_sleep = 0; |
rvasquez6089 | 0:39bb015c8ac2 | 11 | DigitalOut CSENSE_EN(PC_9); |
rvasquez6089 | 0:39bb015c8ac2 | 12 | AnalogIn CSENSE(PC_0); |
rvasquez6089 | 0:39bb015c8ac2 | 13 | AnalogIn VBAT(PC_1); |
rvasquez6089 | 0:39bb015c8ac2 | 14 | DigitalOut SEL1(PA_11); |
rvasquez6089 | 0:39bb015c8ac2 | 15 | DigitalOut SEL2(PA_12); |
rvasquez6089 | 0:39bb015c8ac2 | 16 | void run_300(); |
rvasquez6089 | 0:39bb015c8ac2 | 17 | Timeout go_sleep; |
rvasquez6089 | 0:39bb015c8ac2 | 18 | |
rvasquez6089 | 0:39bb015c8ac2 | 19 | |
rvasquez6089 | 0:39bb015c8ac2 | 20 | bool battery_status() |
rvasquez6089 | 0:39bb015c8ac2 | 21 | { |
rvasquez6089 | 0:39bb015c8ac2 | 22 | double battery_level = 0.0; |
rvasquez6089 | 0:39bb015c8ac2 | 23 | const int on = 50; |
rvasquez6089 | 0:39bb015c8ac2 | 24 | const int off = 100; |
rvasquez6089 | 0:39bb015c8ac2 | 25 | bool low_bat = 0; |
rvasquez6089 | 0:39bb015c8ac2 | 26 | for(int i = 0; i < 1000; i++) |
rvasquez6089 | 0:39bb015c8ac2 | 27 | { |
rvasquez6089 | 0:39bb015c8ac2 | 28 | battery_level = battery_level + VBAT.read(); |
rvasquez6089 | 0:39bb015c8ac2 | 29 | } |
rvasquez6089 | 0:39bb015c8ac2 | 30 | battery_level = battery_level/1000.0; |
rvasquez6089 | 0:39bb015c8ac2 | 31 | battery_level = battery_level*3.3*2.0; |
rvasquez6089 | 0:39bb015c8ac2 | 32 | if(battery_level >= 3.825) |
rvasquez6089 | 0:39bb015c8ac2 | 33 | { |
rvasquez6089 | 0:39bb015c8ac2 | 34 | wait_ms(250); |
rvasquez6089 | 0:39bb015c8ac2 | 35 | Orange = 1; |
rvasquez6089 | 0:39bb015c8ac2 | 36 | wait_ms(on); |
rvasquez6089 | 0:39bb015c8ac2 | 37 | Orange = 0; |
rvasquez6089 | 0:39bb015c8ac2 | 38 | wait_ms(off); |
rvasquez6089 | 0:39bb015c8ac2 | 39 | Orange = 1; |
rvasquez6089 | 0:39bb015c8ac2 | 40 | wait_ms(on); |
rvasquez6089 | 0:39bb015c8ac2 | 41 | Orange = 0; |
rvasquez6089 | 0:39bb015c8ac2 | 42 | wait_ms(off); |
rvasquez6089 | 0:39bb015c8ac2 | 43 | Orange = 1; |
rvasquez6089 | 0:39bb015c8ac2 | 44 | wait_ms(on); |
rvasquez6089 | 0:39bb015c8ac2 | 45 | Orange = 0; |
rvasquez6089 | 0:39bb015c8ac2 | 46 | wait_ms(off); |
rvasquez6089 | 0:39bb015c8ac2 | 47 | Orange = 1; |
rvasquez6089 | 0:39bb015c8ac2 | 48 | wait_ms(on); |
rvasquez6089 | 0:39bb015c8ac2 | 49 | Orange = 0; |
rvasquez6089 | 0:39bb015c8ac2 | 50 | wait_ms(off); |
rvasquez6089 | 0:39bb015c8ac2 | 51 | low_bat = 0; |
rvasquez6089 | 0:39bb015c8ac2 | 52 | } |
rvasquez6089 | 0:39bb015c8ac2 | 53 | if(battery_level >= 3.45 && battery_level < 3.825) |
rvasquez6089 | 0:39bb015c8ac2 | 54 | { |
rvasquez6089 | 0:39bb015c8ac2 | 55 | wait_ms(250); |
rvasquez6089 | 0:39bb015c8ac2 | 56 | Orange = 1; |
rvasquez6089 | 0:39bb015c8ac2 | 57 | wait_ms(on); |
rvasquez6089 | 0:39bb015c8ac2 | 58 | Orange = 0; |
rvasquez6089 | 0:39bb015c8ac2 | 59 | wait_ms(off); |
rvasquez6089 | 0:39bb015c8ac2 | 60 | Orange = 1; |
rvasquez6089 | 0:39bb015c8ac2 | 61 | wait_ms(on); |
rvasquez6089 | 0:39bb015c8ac2 | 62 | Orange = 0; |
rvasquez6089 | 0:39bb015c8ac2 | 63 | wait_ms(off); |
rvasquez6089 | 0:39bb015c8ac2 | 64 | Orange = 1; |
rvasquez6089 | 0:39bb015c8ac2 | 65 | wait_ms(on); |
rvasquez6089 | 0:39bb015c8ac2 | 66 | Orange = 0; |
rvasquez6089 | 0:39bb015c8ac2 | 67 | wait_ms(off); |
rvasquez6089 | 0:39bb015c8ac2 | 68 | low_bat = 0; |
rvasquez6089 | 0:39bb015c8ac2 | 69 | } |
rvasquez6089 | 0:39bb015c8ac2 | 70 | if(battery_level >= 3.075 && battery_level < 3.45) |
rvasquez6089 | 0:39bb015c8ac2 | 71 | { |
rvasquez6089 | 0:39bb015c8ac2 | 72 | wait_ms(250); |
rvasquez6089 | 0:39bb015c8ac2 | 73 | Orange = 1; |
rvasquez6089 | 0:39bb015c8ac2 | 74 | wait_ms(on); |
rvasquez6089 | 0:39bb015c8ac2 | 75 | Orange = 0; |
rvasquez6089 | 0:39bb015c8ac2 | 76 | wait_ms(off); |
rvasquez6089 | 0:39bb015c8ac2 | 77 | Orange = 1; |
rvasquez6089 | 0:39bb015c8ac2 | 78 | wait_ms(on); |
rvasquez6089 | 0:39bb015c8ac2 | 79 | Orange = 0; |
rvasquez6089 | 0:39bb015c8ac2 | 80 | wait_ms(off); |
rvasquez6089 | 0:39bb015c8ac2 | 81 | low_bat = 0; |
rvasquez6089 | 0:39bb015c8ac2 | 82 | } |
rvasquez6089 | 0:39bb015c8ac2 | 83 | if(battery_level < 3.075 && battery_level >= 2.9) |
rvasquez6089 | 0:39bb015c8ac2 | 84 | { |
rvasquez6089 | 0:39bb015c8ac2 | 85 | wait_ms(250); |
rvasquez6089 | 0:39bb015c8ac2 | 86 | Orange = 1; |
rvasquez6089 | 0:39bb015c8ac2 | 87 | wait_ms(on); |
rvasquez6089 | 0:39bb015c8ac2 | 88 | Orange = 0; |
rvasquez6089 | 0:39bb015c8ac2 | 89 | wait_ms(off); |
rvasquez6089 | 0:39bb015c8ac2 | 90 | low_bat = 0; |
rvasquez6089 | 0:39bb015c8ac2 | 91 | } |
rvasquez6089 | 0:39bb015c8ac2 | 92 | if(battery_level < 2.9) |
rvasquez6089 | 0:39bb015c8ac2 | 93 | { |
rvasquez6089 | 0:39bb015c8ac2 | 94 | wait_ms(250); |
rvasquez6089 | 0:39bb015c8ac2 | 95 | Orange = 1; |
rvasquez6089 | 0:39bb015c8ac2 | 96 | wait_ms(75); |
rvasquez6089 | 0:39bb015c8ac2 | 97 | Orange = 0; |
rvasquez6089 | 0:39bb015c8ac2 | 98 | wait_ms(150); |
rvasquez6089 | 0:39bb015c8ac2 | 99 | low_bat = 1; |
rvasquez6089 | 0:39bb015c8ac2 | 100 | } |
rvasquez6089 | 0:39bb015c8ac2 | 101 | return low_bat; |
rvasquez6089 | 0:39bb015c8ac2 | 102 | } |
rvasquez6089 | 0:39bb015c8ac2 | 103 | |
rvasquez6089 | 0:39bb015c8ac2 | 104 | void pressed() |
rvasquez6089 | 0:39bb015c8ac2 | 105 | { |
rvasquez6089 | 0:39bb015c8ac2 | 106 | printf("Button pressed\n"); |
rvasquez6089 | 0:39bb015c8ac2 | 107 | //battery_status(); |
rvasquez6089 | 0:39bb015c8ac2 | 108 | go_to_sleep = !go_to_sleep; |
rvasquez6089 | 0:39bb015c8ac2 | 109 | if(go_to_sleep == 1) |
rvasquez6089 | 0:39bb015c8ac2 | 110 | { |
rvasquez6089 | 0:39bb015c8ac2 | 111 | //battery_status(); |
rvasquez6089 | 0:39bb015c8ac2 | 112 | go_sleep.detach(); |
rvasquez6089 | 0:39bb015c8ac2 | 113 | //Orange = 0.0f; |
rvasquez6089 | 0:39bb015c8ac2 | 114 | } |
rvasquez6089 | 0:39bb015c8ac2 | 115 | if(go_to_sleep == 0) |
rvasquez6089 | 0:39bb015c8ac2 | 116 | { |
rvasquez6089 | 0:39bb015c8ac2 | 117 | if(!(battery_status())) |
rvasquez6089 | 0:39bb015c8ac2 | 118 | { |
rvasquez6089 | 0:39bb015c8ac2 | 119 | go_sleep.attach(&pressed, 10.0); |
rvasquez6089 | 0:39bb015c8ac2 | 120 | } |
rvasquez6089 | 0:39bb015c8ac2 | 121 | } |
rvasquez6089 | 0:39bb015c8ac2 | 122 | } |
rvasquez6089 | 0:39bb015c8ac2 | 123 | |
rvasquez6089 | 0:39bb015c8ac2 | 124 | void run_300_F() |
rvasquez6089 | 0:39bb015c8ac2 | 125 | { |
rvasquez6089 | 0:39bb015c8ac2 | 126 | |
rvasquez6089 | 0:39bb015c8ac2 | 127 | float current; |
rvasquez6089 | 0:39bb015c8ac2 | 128 | int output; |
rvasquez6089 | 0:39bb015c8ac2 | 129 | MOT_D = 0.0; |
rvasquez6089 | 0:39bb015c8ac2 | 130 | MOT_A = 0; |
rvasquez6089 | 0:39bb015c8ac2 | 131 | MOT_B = 1; |
rvasquez6089 | 0:39bb015c8ac2 | 132 | current = 0.0; |
rvasquez6089 | 0:39bb015c8ac2 | 133 | for(int i = 0; i < 10; i++) |
rvasquez6089 | 0:39bb015c8ac2 | 134 | { |
rvasquez6089 | 0:39bb015c8ac2 | 135 | current = current + CSENSE.read(); |
rvasquez6089 | 0:39bb015c8ac2 | 136 | //wait_us(10); |
rvasquez6089 | 0:39bb015c8ac2 | 137 | } |
rvasquez6089 | 0:39bb015c8ac2 | 138 | current = current/10; |
rvasquez6089 | 0:39bb015c8ac2 | 139 | current = ((current*3.3f)/100.0f)/(0.015f); |
rvasquez6089 | 0:39bb015c8ac2 | 140 | current = current*1000.0f; |
rvasquez6089 | 0:39bb015c8ac2 | 141 | output = static_cast<int>(current); |
rvasquez6089 | 0:39bb015c8ac2 | 142 | //pc.printf("Motor Current = %d mA\r\n",output ); |
rvasquez6089 | 0:39bb015c8ac2 | 143 | |
rvasquez6089 | 0:39bb015c8ac2 | 144 | //wait(0.001); |
rvasquez6089 | 0:39bb015c8ac2 | 145 | if(output >= 300) |
rvasquez6089 | 0:39bb015c8ac2 | 146 | { |
rvasquez6089 | 0:39bb015c8ac2 | 147 | MOT_C = MOT_C - (0.01f*(((output-300)/1))); |
rvasquez6089 | 0:39bb015c8ac2 | 148 | } |
rvasquez6089 | 0:39bb015c8ac2 | 149 | if(output < 300) |
rvasquez6089 | 0:39bb015c8ac2 | 150 | { |
rvasquez6089 | 0:39bb015c8ac2 | 151 | MOT_C = 0.9f; |
rvasquez6089 | 0:39bb015c8ac2 | 152 | } |
rvasquez6089 | 0:39bb015c8ac2 | 153 | } |
rvasquez6089 | 0:39bb015c8ac2 | 154 | |
rvasquez6089 | 0:39bb015c8ac2 | 155 | void run_1200_F() |
rvasquez6089 | 0:39bb015c8ac2 | 156 | { |
rvasquez6089 | 0:39bb015c8ac2 | 157 | |
rvasquez6089 | 0:39bb015c8ac2 | 158 | float current; |
rvasquez6089 | 0:39bb015c8ac2 | 159 | int output; |
rvasquez6089 | 0:39bb015c8ac2 | 160 | |
rvasquez6089 | 0:39bb015c8ac2 | 161 | |
rvasquez6089 | 0:39bb015c8ac2 | 162 | MOT_D = 0.0; |
rvasquez6089 | 0:39bb015c8ac2 | 163 | MOT_A = 0; |
rvasquez6089 | 0:39bb015c8ac2 | 164 | MOT_B = 1; |
rvasquez6089 | 0:39bb015c8ac2 | 165 | current = 0.0; |
rvasquez6089 | 0:39bb015c8ac2 | 166 | for(int i = 0; i < 10; i++) |
rvasquez6089 | 0:39bb015c8ac2 | 167 | { |
rvasquez6089 | 0:39bb015c8ac2 | 168 | current = current + CSENSE.read(); |
rvasquez6089 | 0:39bb015c8ac2 | 169 | //wait_us(10); |
rvasquez6089 | 0:39bb015c8ac2 | 170 | } |
rvasquez6089 | 0:39bb015c8ac2 | 171 | current = current/10; |
rvasquez6089 | 0:39bb015c8ac2 | 172 | current = ((current*3.3f)/100.0f)/(0.015f); |
rvasquez6089 | 0:39bb015c8ac2 | 173 | current = current*1000.0f; |
rvasquez6089 | 0:39bb015c8ac2 | 174 | output = static_cast<int>(current); |
rvasquez6089 | 0:39bb015c8ac2 | 175 | //pc.printf("Motor Current = %d mA\r\n",output ); |
rvasquez6089 | 0:39bb015c8ac2 | 176 | |
rvasquez6089 | 0:39bb015c8ac2 | 177 | //wait(0.001); |
rvasquez6089 | 0:39bb015c8ac2 | 178 | if(output >= 1200) |
rvasquez6089 | 0:39bb015c8ac2 | 179 | { |
rvasquez6089 | 0:39bb015c8ac2 | 180 | MOT_C = MOT_C - (0.01f*(((output-1200)/1))); |
rvasquez6089 | 0:39bb015c8ac2 | 181 | } |
rvasquez6089 | 0:39bb015c8ac2 | 182 | if(output < 1200) |
rvasquez6089 | 0:39bb015c8ac2 | 183 | { |
rvasquez6089 | 0:39bb015c8ac2 | 184 | MOT_C = 1.0f; |
rvasquez6089 | 0:39bb015c8ac2 | 185 | } |
rvasquez6089 | 0:39bb015c8ac2 | 186 | } |
rvasquez6089 | 0:39bb015c8ac2 | 187 | |
rvasquez6089 | 0:39bb015c8ac2 | 188 | void run_1500_F() |
rvasquez6089 | 0:39bb015c8ac2 | 189 | { |
rvasquez6089 | 0:39bb015c8ac2 | 190 | |
rvasquez6089 | 0:39bb015c8ac2 | 191 | float current; |
rvasquez6089 | 0:39bb015c8ac2 | 192 | int output; |
rvasquez6089 | 0:39bb015c8ac2 | 193 | |
rvasquez6089 | 0:39bb015c8ac2 | 194 | |
rvasquez6089 | 0:39bb015c8ac2 | 195 | MOT_D = 0.0; |
rvasquez6089 | 0:39bb015c8ac2 | 196 | MOT_A = 0; |
rvasquez6089 | 0:39bb015c8ac2 | 197 | MOT_B = 1; |
rvasquez6089 | 0:39bb015c8ac2 | 198 | current = 0.0; |
rvasquez6089 | 0:39bb015c8ac2 | 199 | for(int i = 0; i < 10; i++) |
rvasquez6089 | 0:39bb015c8ac2 | 200 | { |
rvasquez6089 | 0:39bb015c8ac2 | 201 | current = current + CSENSE.read(); |
rvasquez6089 | 0:39bb015c8ac2 | 202 | //wait_us(10); |
rvasquez6089 | 0:39bb015c8ac2 | 203 | } |
rvasquez6089 | 0:39bb015c8ac2 | 204 | current = current/10; |
rvasquez6089 | 0:39bb015c8ac2 | 205 | current = ((current*3.3f)/100.0f)/(0.015f); |
rvasquez6089 | 0:39bb015c8ac2 | 206 | current = current*1000.0f; |
rvasquez6089 | 0:39bb015c8ac2 | 207 | output = static_cast<int>(current); |
rvasquez6089 | 0:39bb015c8ac2 | 208 | //pc.printf("Motor Current = %d mA\r\n",output ); |
rvasquez6089 | 0:39bb015c8ac2 | 209 | |
rvasquez6089 | 0:39bb015c8ac2 | 210 | //wait(0.001); |
rvasquez6089 | 0:39bb015c8ac2 | 211 | if(output >= 1500) |
rvasquez6089 | 0:39bb015c8ac2 | 212 | { |
rvasquez6089 | 0:39bb015c8ac2 | 213 | MOT_C = MOT_C - (0.01f*(((output-1500)/1))); |
rvasquez6089 | 0:39bb015c8ac2 | 214 | } |
rvasquez6089 | 0:39bb015c8ac2 | 215 | if(output < 1500) |
rvasquez6089 | 0:39bb015c8ac2 | 216 | { |
rvasquez6089 | 0:39bb015c8ac2 | 217 | MOT_C = 1.0f; |
rvasquez6089 | 0:39bb015c8ac2 | 218 | } |
rvasquez6089 | 0:39bb015c8ac2 | 219 | } |
rvasquez6089 | 0:39bb015c8ac2 | 220 | |
rvasquez6089 | 0:39bb015c8ac2 | 221 | |
rvasquez6089 | 0:39bb015c8ac2 | 222 | void run_300_R() |
rvasquez6089 | 0:39bb015c8ac2 | 223 | { |
rvasquez6089 | 0:39bb015c8ac2 | 224 | |
rvasquez6089 | 0:39bb015c8ac2 | 225 | float current; |
rvasquez6089 | 0:39bb015c8ac2 | 226 | int output; |
rvasquez6089 | 0:39bb015c8ac2 | 227 | MOT_B = 0.0; |
rvasquez6089 | 0:39bb015c8ac2 | 228 | MOT_C = 0; |
rvasquez6089 | 0:39bb015c8ac2 | 229 | MOT_A = 1; |
rvasquez6089 | 0:39bb015c8ac2 | 230 | current = 0.0; |
rvasquez6089 | 0:39bb015c8ac2 | 231 | for(int i = 0; i < 10; i++) |
rvasquez6089 | 0:39bb015c8ac2 | 232 | { |
rvasquez6089 | 0:39bb015c8ac2 | 233 | current = current + CSENSE.read(); |
rvasquez6089 | 0:39bb015c8ac2 | 234 | //wait_us(10); |
rvasquez6089 | 0:39bb015c8ac2 | 235 | } |
rvasquez6089 | 0:39bb015c8ac2 | 236 | current = current/10; |
rvasquez6089 | 0:39bb015c8ac2 | 237 | current = ((current*3.3f)/100.0f)/(0.015f); |
rvasquez6089 | 0:39bb015c8ac2 | 238 | current = current*1000.0f; |
rvasquez6089 | 0:39bb015c8ac2 | 239 | output = static_cast<int>(current); |
rvasquez6089 | 0:39bb015c8ac2 | 240 | //pc.printf("Motor Current = %d mA\r\n",output ); |
rvasquez6089 | 0:39bb015c8ac2 | 241 | |
rvasquez6089 | 0:39bb015c8ac2 | 242 | //wait(0.001); |
rvasquez6089 | 0:39bb015c8ac2 | 243 | if(output >= 300) |
rvasquez6089 | 0:39bb015c8ac2 | 244 | { |
rvasquez6089 | 0:39bb015c8ac2 | 245 | MOT_D = MOT_D - (0.01f*(((output-300)/1))); |
rvasquez6089 | 0:39bb015c8ac2 | 246 | } |
rvasquez6089 | 0:39bb015c8ac2 | 247 | if(output < 300) |
rvasquez6089 | 0:39bb015c8ac2 | 248 | { |
rvasquez6089 | 0:39bb015c8ac2 | 249 | MOT_D = 0.9f; |
rvasquez6089 | 0:39bb015c8ac2 | 250 | } |
rvasquez6089 | 0:39bb015c8ac2 | 251 | } |
rvasquez6089 | 0:39bb015c8ac2 | 252 | |
rvasquez6089 | 0:39bb015c8ac2 | 253 | void run_1500_R() |
rvasquez6089 | 0:39bb015c8ac2 | 254 | { |
rvasquez6089 | 0:39bb015c8ac2 | 255 | |
rvasquez6089 | 0:39bb015c8ac2 | 256 | float current; |
rvasquez6089 | 0:39bb015c8ac2 | 257 | int output; |
rvasquez6089 | 0:39bb015c8ac2 | 258 | MOT_B = 0.0; |
rvasquez6089 | 0:39bb015c8ac2 | 259 | MOT_C = 0; |
rvasquez6089 | 0:39bb015c8ac2 | 260 | MOT_A = 1; |
rvasquez6089 | 0:39bb015c8ac2 | 261 | current = 0.0; |
rvasquez6089 | 0:39bb015c8ac2 | 262 | for(int i = 0; i < 10; i++) |
rvasquez6089 | 0:39bb015c8ac2 | 263 | { |
rvasquez6089 | 0:39bb015c8ac2 | 264 | current = current + CSENSE.read(); |
rvasquez6089 | 0:39bb015c8ac2 | 265 | //wait_us(10); |
rvasquez6089 | 0:39bb015c8ac2 | 266 | } |
rvasquez6089 | 0:39bb015c8ac2 | 267 | current = current/10; |
rvasquez6089 | 0:39bb015c8ac2 | 268 | current = ((current*3.3f)/100.0f)/(0.015f); |
rvasquez6089 | 0:39bb015c8ac2 | 269 | current = current*1000.0f; |
rvasquez6089 | 0:39bb015c8ac2 | 270 | output = static_cast<int>(current); |
rvasquez6089 | 0:39bb015c8ac2 | 271 | //pc.printf("Motor Current = %d mA\r\n",output ); |
rvasquez6089 | 0:39bb015c8ac2 | 272 | |
rvasquez6089 | 0:39bb015c8ac2 | 273 | //wait(0.001); |
rvasquez6089 | 0:39bb015c8ac2 | 274 | if(output >= 1500) |
rvasquez6089 | 0:39bb015c8ac2 | 275 | { |
rvasquez6089 | 0:39bb015c8ac2 | 276 | MOT_D = MOT_D - (0.01f*(((output-1500)/1))); |
rvasquez6089 | 0:39bb015c8ac2 | 277 | } |
rvasquez6089 | 0:39bb015c8ac2 | 278 | if(output < 1500) |
rvasquez6089 | 0:39bb015c8ac2 | 279 | { |
rvasquez6089 | 0:39bb015c8ac2 | 280 | MOT_D = 1.0f; |
rvasquez6089 | 0:39bb015c8ac2 | 281 | } |
rvasquez6089 | 0:39bb015c8ac2 | 282 | } |
rvasquez6089 | 0:39bb015c8ac2 | 283 | |
rvasquez6089 | 0:39bb015c8ac2 | 284 | |
rvasquez6089 | 0:39bb015c8ac2 | 285 | int main() |
rvasquez6089 | 0:39bb015c8ac2 | 286 | { |
rvasquez6089 | 0:39bb015c8ac2 | 287 | int i = 0; |
rvasquez6089 | 0:39bb015c8ac2 | 288 | MOT_A = 0; |
rvasquez6089 | 0:39bb015c8ac2 | 289 | MOT_B = 0; |
rvasquez6089 | 0:39bb015c8ac2 | 290 | MOT_C = 0.0f; |
rvasquez6089 | 0:39bb015c8ac2 | 291 | MOT_D = 0.0f; |
rvasquez6089 | 0:39bb015c8ac2 | 292 | //Orange = 0.0f; |
rvasquez6089 | 0:39bb015c8ac2 | 293 | event.fall(&pressed); |
rvasquez6089 | 0:39bb015c8ac2 | 294 | battery_status(); |
rvasquez6089 | 0:39bb015c8ac2 | 295 | MOT_C.period(0.00005); |
rvasquez6089 | 0:39bb015c8ac2 | 296 | MOT_D.period(0.00005); |
rvasquez6089 | 0:39bb015c8ac2 | 297 | //Orange.period(0.005); |
rvasquez6089 | 0:39bb015c8ac2 | 298 | go_to_sleep = 1; |
rvasquez6089 | 0:39bb015c8ac2 | 299 | while (1) { |
rvasquez6089 | 0:39bb015c8ac2 | 300 | if (go_to_sleep) { |
rvasquez6089 | 0:39bb015c8ac2 | 301 | |
rvasquez6089 | 0:39bb015c8ac2 | 302 | myled = 0; |
rvasquez6089 | 0:39bb015c8ac2 | 303 | //printf("%d: Entering sleep (press user button to resume)\n", i); |
rvasquez6089 | 0:39bb015c8ac2 | 304 | Motor.detach(); |
rvasquez6089 | 0:39bb015c8ac2 | 305 | SEL1 = 0; |
rvasquez6089 | 0:39bb015c8ac2 | 306 | SEL2 = 0; |
rvasquez6089 | 0:39bb015c8ac2 | 307 | CSENSE_EN = 0; |
rvasquez6089 | 0:39bb015c8ac2 | 308 | Orange = 0; |
rvasquez6089 | 0:39bb015c8ac2 | 309 | MOT_D = 0.0f; |
rvasquez6089 | 0:39bb015c8ac2 | 310 | MOT_A = 0; |
rvasquez6089 | 0:39bb015c8ac2 | 311 | MOT_C = 0.0f; |
rvasquez6089 | 0:39bb015c8ac2 | 312 | MOT_B = 0; |
rvasquez6089 | 0:39bb015c8ac2 | 313 | //Orange = 0.0f; |
rvasquez6089 | 0:39bb015c8ac2 | 314 | sleep(); |
rvasquez6089 | 0:39bb015c8ac2 | 315 | //deepsleep(); |
rvasquez6089 | 0:39bb015c8ac2 | 316 | wait(0.1); |
rvasquez6089 | 0:39bb015c8ac2 | 317 | SEL1=1; |
rvasquez6089 | 0:39bb015c8ac2 | 318 | SEL2=1; |
rvasquez6089 | 0:39bb015c8ac2 | 319 | CSENSE_EN = 1; |
rvasquez6089 | 0:39bb015c8ac2 | 320 | } else { |
rvasquez6089 | 0:39bb015c8ac2 | 321 | //printf("%d: Running\n", i); |
rvasquez6089 | 0:39bb015c8ac2 | 322 | myled = 1; |
rvasquez6089 | 0:39bb015c8ac2 | 323 | Orange = 1; |
rvasquez6089 | 0:39bb015c8ac2 | 324 | run_1500_R(); |
rvasquez6089 | 0:39bb015c8ac2 | 325 | |
rvasquez6089 | 0:39bb015c8ac2 | 326 | } |
rvasquez6089 | 0:39bb015c8ac2 | 327 | i++; |
rvasquez6089 | 0:39bb015c8ac2 | 328 | } |
rvasquez6089 | 0:39bb015c8ac2 | 329 | } |