Mouse code for the MacroRat
main.cpp@25:f827a8b7880e, 2017-05-21 (annotated)
- Committer:
- christine222
- Date:
- Sun May 21 08:52:43 2017 +0000
- Revision:
- 25:f827a8b7880e
- Parent:
- 24:e7063765d6f0
- Child:
- 26:d20f1adac2d3
180 in the beginning
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
sahilmgandhi | 0:a03c771ab78e | 1 | #include "mbed.h" |
christine222 | 25:f827a8b7880e | 2 | |
kyleliangus | 6:3d68fedd6fd9 | 3 | #include "irpair.h" |
kyleliangus | 4:b5b7836ca2b0 | 4 | #include "main.h" |
kyleliangus | 4:b5b7836ca2b0 | 5 | #include "motor.h" |
christine222 | 25:f827a8b7880e | 6 | |
sahilmgandhi | 0:a03c771ab78e | 7 | #include <stdlib.h> |
sahilmgandhi | 1:8a4b2f573923 | 8 | #include "ITG3200.h" |
sahilmgandhi | 14:9e7bb03ddccb | 9 | #include "stm32f4xx.h" |
sahilmgandhi | 7:6f5cb6377bd4 | 10 | #include "QEI.h" |
christine222 | 25:f827a8b7880e | 11 | |
vanshg | 11:8fc2b703086b | 12 | /* Constants for when HIGH_PWM_VOLTAGE = 0.2 |
kyleliangus | 9:1d8e4da058cd | 13 | #define IP_CONSTANT 6 |
kyleliangus | 9:1d8e4da058cd | 14 | #define II_CONSTANT 0 |
kyleliangus | 9:1d8e4da058cd | 15 | #define ID_CONSTANT 1 |
vanshg | 11:8fc2b703086b | 16 | */ |
christine222 | 25:f827a8b7880e | 17 | |
vanshg | 11:8fc2b703086b | 18 | // Constants for when HIGH_PWM_VOLTAGE = 0.1 |
christine222 | 21:9a6cb07bdcb6 | 19 | // #define IP_CONSTANT 8.85 |
christine222 | 21:9a6cb07bdcb6 | 20 | // #define II_CONSTANT 0.005 |
christine222 | 21:9a6cb07bdcb6 | 21 | // #define ID_CONSTANT 3.15 |
christine222 | 25:f827a8b7880e | 22 | #define IP_CONSTANT 8.5 |
christine222 | 21:9a6cb07bdcb6 | 23 | #define II_CONSTANT 0.095 |
christine222 | 25:f827a8b7880e | 24 | #define ID_CONSTANT 6.85 |
christine222 | 25:f827a8b7880e | 25 | |
christine222 | 25:f827a8b7880e | 26 | const int desiredCount180 = 2850; |
christine222 | 25:f827a8b7880e | 27 | const int desiredCountR = 1575; |
christine222 | 25:f827a8b7880e | 28 | const int desiredCountL = 1650; |
christine222 | 25:f827a8b7880e | 29 | |
sahilmgandhi | 16:d9252437bd92 | 30 | const int oneCellCount = 5400; |
christine222 | 25:f827a8b7880e | 31 | const int oneCellCountMomentum = 4700; // one cell count is actually approximately 5400, but this value is considering momentum! |
christine222 | 25:f827a8b7880e | 32 | |
sahilmgandhi | 17:f713758f6238 | 33 | float receiverOneReading = 0.0; |
sahilmgandhi | 17:f713758f6238 | 34 | float receiverTwoReading = 0.0; |
sahilmgandhi | 17:f713758f6238 | 35 | float receiverThreeReading = 0.0; |
sahilmgandhi | 17:f713758f6238 | 36 | float receiverFourReading = 0.0; |
sahilmgandhi | 17:f713758f6238 | 37 | |
christine222 | 23:690b0ca34ee9 | 38 | |
christine222 | 25:f827a8b7880e | 39 | float initAverageL = 8.28; |
christine222 | 25:f827a8b7880e | 40 | float averageDivL = 29.5; //blue |
christine222 | 25:f827a8b7880e | 41 | float initAverageR = 8.75; //4.5 |
christine222 | 25:f827a8b7880e | 42 | float averageDivR = 29.5; //red |
christine222 | 25:f827a8b7880e | 43 | float averageDivUpper = 0.9; |
christine222 | 25:f827a8b7880e | 44 | |
sahilmgandhi | 16:d9252437bd92 | 45 | void pidOnEncoders(); |
christine222 | 25:f827a8b7880e | 46 | |
christine222 | 25:f827a8b7880e | 47 | |
sahilmgandhi | 14:9e7bb03ddccb | 48 | void turnLeft() |
sahilmgandhi | 14:9e7bb03ddccb | 49 | { |
christine222 | 23:690b0ca34ee9 | 50 | double speed0 = 0.11; |
christine222 | 23:690b0ca34ee9 | 51 | double speed1 = -0.13; |
christine222 | 25:f827a8b7880e | 52 | |
christine222 | 13:2032db00f168 | 53 | int counter = 0; |
sahilmgandhi | 14:9e7bb03ddccb | 54 | int initial0 = encoder0.getPulses(); |
sahilmgandhi | 14:9e7bb03ddccb | 55 | int initial1 = encoder1.getPulses(); |
christine222 | 25:f827a8b7880e | 56 | |
sahilmgandhi | 14:9e7bb03ddccb | 57 | int desiredCount0 = initial0 - desiredCountL; |
sahilmgandhi | 14:9e7bb03ddccb | 58 | int desiredCount1 = initial1 + desiredCountL; |
christine222 | 25:f827a8b7880e | 59 | |
sahilmgandhi | 14:9e7bb03ddccb | 60 | int count0 = initial0; |
sahilmgandhi | 14:9e7bb03ddccb | 61 | int count1 = initial1; |
christine222 | 25:f827a8b7880e | 62 | |
christine222 | 13:2032db00f168 | 63 | double error0 = count0 - desiredCount0; |
christine222 | 13:2032db00f168 | 64 | double error1 = count1 - desiredCount1; |
christine222 | 25:f827a8b7880e | 65 | |
christine222 | 25:f827a8b7880e | 66 | |
sahilmgandhi | 14:9e7bb03ddccb | 67 | while(1) { |
christine222 | 25:f827a8b7880e | 68 | |
sahilmgandhi | 14:9e7bb03ddccb | 69 | if(!(abs(error0) < 1) && !(abs(error1) < 1)) { |
christine222 | 13:2032db00f168 | 70 | count0 = encoder0.getPulses(); |
christine222 | 13:2032db00f168 | 71 | count1 = encoder1.getPulses(); |
christine222 | 25:f827a8b7880e | 72 | |
sahilmgandhi | 14:9e7bb03ddccb | 73 | error0 = count0 - desiredCount0; |
sahilmgandhi | 14:9e7bb03ddccb | 74 | error1 = count1 - desiredCount1; |
christine222 | 25:f827a8b7880e | 75 | |
christine222 | 25:f827a8b7880e | 76 | right_motor.move(speed0); |
christine222 | 25:f827a8b7880e | 77 | left_motor.move(speed1); |
christine222 | 25:f827a8b7880e | 78 | counter = 0; |
christine222 | 25:f827a8b7880e | 79 | } else { |
christine222 | 25:f827a8b7880e | 80 | counter++; |
christine222 | 25:f827a8b7880e | 81 | right_motor.brake(); |
christine222 | 25:f827a8b7880e | 82 | left_motor.brake(); |
christine222 | 25:f827a8b7880e | 83 | } |
christine222 | 25:f827a8b7880e | 84 | |
christine222 | 25:f827a8b7880e | 85 | if (counter > 60) { |
christine222 | 25:f827a8b7880e | 86 | break; |
christine222 | 25:f827a8b7880e | 87 | } |
christine222 | 25:f827a8b7880e | 88 | } |
christine222 | 25:f827a8b7880e | 89 | |
christine222 | 25:f827a8b7880e | 90 | right_motor.brake(); |
christine222 | 25:f827a8b7880e | 91 | left_motor.brake(); |
christine222 | 25:f827a8b7880e | 92 | turnFlag = 0; // zeroing out the flags! |
christine222 | 25:f827a8b7880e | 93 | currDir -= 1; |
christine222 | 25:f827a8b7880e | 94 | } |
christine222 | 25:f827a8b7880e | 95 | |
christine222 | 25:f827a8b7880e | 96 | void turnRight() |
christine222 | 25:f827a8b7880e | 97 | { |
christine222 | 25:f827a8b7880e | 98 | double speed0 = -0.11; |
christine222 | 25:f827a8b7880e | 99 | double speed1 = 0.13; |
christine222 | 25:f827a8b7880e | 100 | |
christine222 | 25:f827a8b7880e | 101 | int counter = 0; |
christine222 | 25:f827a8b7880e | 102 | int initial0 = encoder0.getPulses(); |
christine222 | 25:f827a8b7880e | 103 | int initial1 = encoder1.getPulses(); |
christine222 | 25:f827a8b7880e | 104 | |
christine222 | 25:f827a8b7880e | 105 | int desiredCount0 = initial0 + desiredCountR; |
christine222 | 25:f827a8b7880e | 106 | int desiredCount1 = initial1 - desiredCountR; |
christine222 | 25:f827a8b7880e | 107 | |
christine222 | 25:f827a8b7880e | 108 | int count0 = initial0; |
christine222 | 25:f827a8b7880e | 109 | int count1 = initial1; |
christine222 | 25:f827a8b7880e | 110 | |
christine222 | 25:f827a8b7880e | 111 | double error0 = count0 - desiredCount0; |
christine222 | 25:f827a8b7880e | 112 | double error1 = count1 - desiredCount1; |
christine222 | 25:f827a8b7880e | 113 | |
christine222 | 25:f827a8b7880e | 114 | while(1) { |
christine222 | 25:f827a8b7880e | 115 | |
christine222 | 25:f827a8b7880e | 116 | if(!(abs(error0) < 1) && !(abs(error1) < 1)) { |
christine222 | 25:f827a8b7880e | 117 | count0 = encoder0.getPulses(); |
christine222 | 25:f827a8b7880e | 118 | count1 = encoder1.getPulses(); |
christine222 | 25:f827a8b7880e | 119 | |
christine222 | 25:f827a8b7880e | 120 | error0 = count0 - desiredCount0; |
christine222 | 25:f827a8b7880e | 121 | error1 = count1 - desiredCount1; |
christine222 | 25:f827a8b7880e | 122 | |
christine222 | 13:2032db00f168 | 123 | right_motor.move(speed0); |
christine222 | 13:2032db00f168 | 124 | left_motor.move(speed1); |
christine222 | 13:2032db00f168 | 125 | counter = 0; |
sahilmgandhi | 14:9e7bb03ddccb | 126 | } else { |
christine222 | 13:2032db00f168 | 127 | counter++; |
christine222 | 13:2032db00f168 | 128 | right_motor.brake(); |
christine222 | 13:2032db00f168 | 129 | left_motor.brake(); |
christine222 | 13:2032db00f168 | 130 | } |
christine222 | 25:f827a8b7880e | 131 | |
sahilmgandhi | 14:9e7bb03ddccb | 132 | if (counter > 60) { |
christine222 | 13:2032db00f168 | 133 | break; |
christine222 | 13:2032db00f168 | 134 | } |
sahilmgandhi | 14:9e7bb03ddccb | 135 | } |
christine222 | 25:f827a8b7880e | 136 | |
christine222 | 13:2032db00f168 | 137 | right_motor.brake(); |
christine222 | 13:2032db00f168 | 138 | left_motor.brake(); |
christine222 | 25:f827a8b7880e | 139 | turnFlag = 0; |
christine222 | 25:f827a8b7880e | 140 | currDir += 1; |
christine222 | 13:2032db00f168 | 141 | } |
christine222 | 25:f827a8b7880e | 142 | |
christine222 | 25:f827a8b7880e | 143 | void turnLeft180() |
sahilmgandhi | 14:9e7bb03ddccb | 144 | { |
christine222 | 25:f827a8b7880e | 145 | double speed0 = 0.15; |
christine222 | 25:f827a8b7880e | 146 | double speed1 = -0.15; |
christine222 | 25:f827a8b7880e | 147 | |
christine222 | 13:2032db00f168 | 148 | int counter = 0; |
sahilmgandhi | 14:9e7bb03ddccb | 149 | int initial0 = encoder0.getPulses(); |
sahilmgandhi | 14:9e7bb03ddccb | 150 | int initial1 = encoder1.getPulses(); |
christine222 | 25:f827a8b7880e | 151 | |
christine222 | 25:f827a8b7880e | 152 | int desiredCount0 = initial0 - desiredCountL*2; |
christine222 | 25:f827a8b7880e | 153 | int desiredCount1 = initial1 + desiredCountL*2; |
christine222 | 25:f827a8b7880e | 154 | |
sahilmgandhi | 14:9e7bb03ddccb | 155 | int count0 = initial0; |
sahilmgandhi | 14:9e7bb03ddccb | 156 | int count1 = initial1; |
christine222 | 25:f827a8b7880e | 157 | |
christine222 | 13:2032db00f168 | 158 | double error0 = count0 - desiredCount0; |
christine222 | 13:2032db00f168 | 159 | double error1 = count1 - desiredCount1; |
christine222 | 25:f827a8b7880e | 160 | |
christine222 | 25:f827a8b7880e | 161 | |
sahilmgandhi | 14:9e7bb03ddccb | 162 | while(1) { |
christine222 | 25:f827a8b7880e | 163 | |
sahilmgandhi | 14:9e7bb03ddccb | 164 | if(!(abs(error0) < 1) && !(abs(error1) < 1)) { |
christine222 | 13:2032db00f168 | 165 | count0 = encoder0.getPulses(); |
christine222 | 13:2032db00f168 | 166 | count1 = encoder1.getPulses(); |
christine222 | 25:f827a8b7880e | 167 | |
sahilmgandhi | 14:9e7bb03ddccb | 168 | error0 = count0 - desiredCount0; |
sahilmgandhi | 14:9e7bb03ddccb | 169 | error1 = count1 - desiredCount1; |
christine222 | 25:f827a8b7880e | 170 | |
christine222 | 13:2032db00f168 | 171 | right_motor.move(speed0); |
christine222 | 13:2032db00f168 | 172 | left_motor.move(speed1); |
christine222 | 13:2032db00f168 | 173 | counter = 0; |
sahilmgandhi | 14:9e7bb03ddccb | 174 | } else { |
christine222 | 13:2032db00f168 | 175 | counter++; |
christine222 | 13:2032db00f168 | 176 | right_motor.brake(); |
christine222 | 13:2032db00f168 | 177 | left_motor.brake(); |
christine222 | 13:2032db00f168 | 178 | } |
christine222 | 25:f827a8b7880e | 179 | |
sahilmgandhi | 14:9e7bb03ddccb | 180 | if (counter > 60) { |
christine222 | 13:2032db00f168 | 181 | break; |
christine222 | 13:2032db00f168 | 182 | } |
christine222 | 13:2032db00f168 | 183 | } |
christine222 | 25:f827a8b7880e | 184 | |
christine222 | 13:2032db00f168 | 185 | right_motor.brake(); |
christine222 | 13:2032db00f168 | 186 | left_motor.brake(); |
christine222 | 25:f827a8b7880e | 187 | currDir -= 2; |
christine222 | 13:2032db00f168 | 188 | } |
christine222 | 25:f827a8b7880e | 189 | |
christine222 | 25:f827a8b7880e | 190 | void turnRight180() |
sahilmgandhi | 14:9e7bb03ddccb | 191 | { |
christine222 | 25:f827a8b7880e | 192 | double speed0 = -0.16; |
christine222 | 25:f827a8b7880e | 193 | double speed1 = 0.16; |
christine222 | 25:f827a8b7880e | 194 | |
christine222 | 13:2032db00f168 | 195 | int counter = 0; |
sahilmgandhi | 14:9e7bb03ddccb | 196 | int initial0 = encoder0.getPulses(); |
sahilmgandhi | 14:9e7bb03ddccb | 197 | int initial1 = encoder1.getPulses(); |
christine222 | 25:f827a8b7880e | 198 | |
christine222 | 25:f827a8b7880e | 199 | int desiredCount0 = initial0 + desiredCount180; |
christine222 | 25:f827a8b7880e | 200 | int desiredCount1 = initial1 - desiredCount180; |
christine222 | 25:f827a8b7880e | 201 | |
sahilmgandhi | 14:9e7bb03ddccb | 202 | int count0 = initial0; |
sahilmgandhi | 14:9e7bb03ddccb | 203 | int count1 = initial1; |
christine222 | 25:f827a8b7880e | 204 | |
christine222 | 13:2032db00f168 | 205 | double error0 = count0 - desiredCount0; |
christine222 | 13:2032db00f168 | 206 | double error1 = count1 - desiredCount1; |
christine222 | 25:f827a8b7880e | 207 | |
christine222 | 25:f827a8b7880e | 208 | |
sahilmgandhi | 14:9e7bb03ddccb | 209 | while(1) { |
christine222 | 25:f827a8b7880e | 210 | |
sahilmgandhi | 14:9e7bb03ddccb | 211 | if(!(abs(error0) < 1) && !(abs(error1) < 1)) { |
christine222 | 13:2032db00f168 | 212 | count0 = encoder0.getPulses(); |
christine222 | 13:2032db00f168 | 213 | count1 = encoder1.getPulses(); |
christine222 | 25:f827a8b7880e | 214 | |
sahilmgandhi | 14:9e7bb03ddccb | 215 | error0 = count0 - desiredCount0; |
sahilmgandhi | 14:9e7bb03ddccb | 216 | error1 = count1 - desiredCount1; |
christine222 | 25:f827a8b7880e | 217 | |
christine222 | 13:2032db00f168 | 218 | right_motor.move(speed0); |
christine222 | 13:2032db00f168 | 219 | left_motor.move(speed1); |
christine222 | 13:2032db00f168 | 220 | counter = 0; |
sahilmgandhi | 14:9e7bb03ddccb | 221 | } else { |
christine222 | 13:2032db00f168 | 222 | counter++; |
christine222 | 13:2032db00f168 | 223 | right_motor.brake(); |
christine222 | 13:2032db00f168 | 224 | left_motor.brake(); |
christine222 | 13:2032db00f168 | 225 | } |
christine222 | 25:f827a8b7880e | 226 | |
sahilmgandhi | 14:9e7bb03ddccb | 227 | if (counter > 60) { |
sahilmgandhi | 14:9e7bb03ddccb | 228 | break; |
sahilmgandhi | 14:9e7bb03ddccb | 229 | } |
sahilmgandhi | 14:9e7bb03ddccb | 230 | } |
sahilmgandhi | 14:9e7bb03ddccb | 231 | right_motor.brake(); |
sahilmgandhi | 14:9e7bb03ddccb | 232 | left_motor.brake(); |
sahilmgandhi | 20:82836745332e | 233 | currDir += 2; |
sahilmgandhi | 14:9e7bb03ddccb | 234 | } |
christine222 | 25:f827a8b7880e | 235 | |
sahilmgandhi | 17:f713758f6238 | 236 | void moveForwardCellEncoder(double cellNum){ |
sahilmgandhi | 17:f713758f6238 | 237 | int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellNum; |
sahilmgandhi | 17:f713758f6238 | 238 | int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellNum; |
christine222 | 25:f827a8b7880e | 239 | |
sahilmgandhi | 20:82836745332e | 240 | left_motor.forward(0.125); |
sahilmgandhi | 20:82836745332e | 241 | right_motor.forward(0.095); |
sahilmgandhi | 19:7b66a518b6f8 | 242 | wait_ms(1); |
sahilmgandhi | 16:d9252437bd92 | 243 | while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1){ |
sahilmgandhi | 20:82836745332e | 244 | receiverTwoReading = IRP_2.getSamples(100); |
sahilmgandhi | 20:82836745332e | 245 | receiverThreeReading = IRP_3.getSamples(100); |
sahilmgandhi | 20:82836745332e | 246 | // serial.printf("Average 2: %f Average 3: %f Sensor 2: %f Sensor 3: %f\n", IRP_2.sensorAvg, IRP_3.sensorAvg, receiverTwoReading, receiverThreeReading); |
christine222 | 25:f827a8b7880e | 247 | if (receiverThreeReading < IRP_3.sensorAvg/averageDivR){ |
sahilmgandhi | 20:82836745332e | 248 | // redLed.write(1); |
sahilmgandhi | 20:82836745332e | 249 | // blueLed.write(0); |
sahilmgandhi | 19:7b66a518b6f8 | 250 | turnFlag |= RIGHT_FLAG; |
sahilmgandhi | 20:82836745332e | 251 | } |
christine222 | 25:f827a8b7880e | 252 | else if (receiverTwoReading < IRP_2.sensorAvg/averageDivL){ |
sahilmgandhi | 20:82836745332e | 253 | // redLed.write(0); |
sahilmgandhi | 20:82836745332e | 254 | // blueLed.write(1); |
sahilmgandhi | 17:f713758f6238 | 255 | turnFlag |= LEFT_FLAG; |
sahilmgandhi | 20:82836745332e | 256 | } |
sahilmgandhi | 16:d9252437bd92 | 257 | pidOnEncoders(); |
sahilmgandhi | 16:d9252437bd92 | 258 | } |
christine222 | 25:f827a8b7880e | 259 | |
sahilmgandhi | 16:d9252437bd92 | 260 | left_motor.brake(); |
sahilmgandhi | 14:9e7bb03ddccb | 261 | right_motor.brake(); |
sahilmgandhi | 14:9e7bb03ddccb | 262 | } |
christine222 | 25:f827a8b7880e | 263 | |
sahilmgandhi | 17:f713758f6238 | 264 | void handleTurns(){ |
sahilmgandhi | 17:f713758f6238 | 265 | if (turnFlag == 0x1){ |
sahilmgandhi | 19:7b66a518b6f8 | 266 | // moveForwardCellEncoder(0.3); |
sahilmgandhi | 17:f713758f6238 | 267 | turnLeft(); |
sahilmgandhi | 17:f713758f6238 | 268 | } |
sahilmgandhi | 17:f713758f6238 | 269 | else if (turnFlag == 0x2){ |
sahilmgandhi | 19:7b66a518b6f8 | 270 | // moveForwardCellEncoder(0.3); |
sahilmgandhi | 17:f713758f6238 | 271 | turnRight(); |
sahilmgandhi | 17:f713758f6238 | 272 | } |
sahilmgandhi | 17:f713758f6238 | 273 | else if (turnFlag == 0x3){ |
sahilmgandhi | 19:7b66a518b6f8 | 274 | // moveForwardCellEncoder(0.3); |
sahilmgandhi | 17:f713758f6238 | 275 | turnLeft(); |
sahilmgandhi | 19:7b66a518b6f8 | 276 | turnRight(); |
sahilmgandhi | 17:f713758f6238 | 277 | } |
sahilmgandhi | 17:f713758f6238 | 278 | } |
christine222 | 25:f827a8b7880e | 279 | |
christine222 | 21:9a6cb07bdcb6 | 280 | void pidBrake(){ |
christine222 | 25:f827a8b7880e | 281 | |
christine222 | 21:9a6cb07bdcb6 | 282 | int count0; |
christine222 | 21:9a6cb07bdcb6 | 283 | int count1; |
christine222 | 21:9a6cb07bdcb6 | 284 | count0 = encoder0.getPulses(); |
christine222 | 21:9a6cb07bdcb6 | 285 | count1 = encoder1.getPulses(); |
christine222 | 21:9a6cb07bdcb6 | 286 | int initial0 = count0; |
christine222 | 21:9a6cb07bdcb6 | 287 | int initial1 = count1; |
christine222 | 21:9a6cb07bdcb6 | 288 | double kp = 0.00011; |
christine222 | 25:f827a8b7880e | 289 | |
christine222 | 25:f827a8b7880e | 290 | |
christine222 | 25:f827a8b7880e | 291 | |
christine222 | 21:9a6cb07bdcb6 | 292 | int error = count0 - count1; |
christine222 | 25:f827a8b7880e | 293 | |
christine222 | 21:9a6cb07bdcb6 | 294 | int counter = 0; |
christine222 | 21:9a6cb07bdcb6 | 295 | right_motor.move(0.7); |
christine222 | 21:9a6cb07bdcb6 | 296 | left_motor.move(0.7); |
christine222 | 25:f827a8b7880e | 297 | |
christine222 | 21:9a6cb07bdcb6 | 298 | double speed0 = 0.7; |
christine222 | 21:9a6cb07bdcb6 | 299 | double speed1 = 0.7; |
christine222 | 25:f827a8b7880e | 300 | |
christine222 | 21:9a6cb07bdcb6 | 301 | while(1) |
christine222 | 21:9a6cb07bdcb6 | 302 | { |
christine222 | 21:9a6cb07bdcb6 | 303 | if(abs(error) < 3){ |
christine222 | 21:9a6cb07bdcb6 | 304 | right_motor.brake(); |
christine222 | 21:9a6cb07bdcb6 | 305 | left_motor.brake(); |
christine222 | 21:9a6cb07bdcb6 | 306 | counter++; |
christine222 | 21:9a6cb07bdcb6 | 307 | }else{ |
christine222 | 21:9a6cb07bdcb6 | 308 | count0 = encoder0.getPulses() - initial0; |
christine222 | 21:9a6cb07bdcb6 | 309 | count1 = encoder1.getPulses() - initial1; |
christine222 | 21:9a6cb07bdcb6 | 310 | error = count0 - count1; |
christine222 | 21:9a6cb07bdcb6 | 311 | speed0 = -error*kp + speed0; |
christine222 | 21:9a6cb07bdcb6 | 312 | speed1 = error*kp + speed1; |
christine222 | 25:f827a8b7880e | 313 | |
christine222 | 21:9a6cb07bdcb6 | 314 | right_motor.move(speed0); |
christine222 | 21:9a6cb07bdcb6 | 315 | left_motor.move(speed1); |
christine222 | 25:f827a8b7880e | 316 | |
christine222 | 21:9a6cb07bdcb6 | 317 | counter = 0; |
christine222 | 21:9a6cb07bdcb6 | 318 | } |
christine222 | 21:9a6cb07bdcb6 | 319 | if (counter > 10){ |
christine222 | 21:9a6cb07bdcb6 | 320 | break; |
christine222 | 21:9a6cb07bdcb6 | 321 | } |
christine222 | 25:f827a8b7880e | 322 | |
christine222 | 21:9a6cb07bdcb6 | 323 | } |
christine222 | 21:9a6cb07bdcb6 | 324 | return; |
christine222 | 21:9a6cb07bdcb6 | 325 | } |
christine222 | 25:f827a8b7880e | 326 | |
christine222 | 21:9a6cb07bdcb6 | 327 | void moveForwardEncoder(){ |
christine222 | 25:f827a8b7880e | 328 | |
christine222 | 21:9a6cb07bdcb6 | 329 | int count0; |
christine222 | 21:9a6cb07bdcb6 | 330 | int count1; |
christine222 | 21:9a6cb07bdcb6 | 331 | count0 = encoder0.getPulses(); |
christine222 | 21:9a6cb07bdcb6 | 332 | count1 = encoder1.getPulses(); |
christine222 | 21:9a6cb07bdcb6 | 333 | int initial1 = count1; |
christine222 | 21:9a6cb07bdcb6 | 334 | int initial0 = count0; |
christine222 | 21:9a6cb07bdcb6 | 335 | int diff = count0 - count1; |
christine222 | 21:9a6cb07bdcb6 | 336 | double kp = 0.00015; |
christine222 | 21:9a6cb07bdcb6 | 337 | double kd = 0.00019; |
christine222 | 21:9a6cb07bdcb6 | 338 | int prev = 0; |
christine222 | 25:f827a8b7880e | 339 | |
christine222 | 25:f827a8b7880e | 340 | |
christine222 | 25:f827a8b7880e | 341 | |
christine222 | 23:690b0ca34ee9 | 342 | double speed0 = 0.10; |
christine222 | 23:690b0ca34ee9 | 343 | double speed1 = 0.12; |
christine222 | 21:9a6cb07bdcb6 | 344 | right_motor.move(speed0); |
christine222 | 21:9a6cb07bdcb6 | 345 | left_motor.move(speed1); |
christine222 | 25:f827a8b7880e | 346 | |
christine222 | 25:f827a8b7880e | 347 | |
christine222 | 25:f827a8b7880e | 348 | while((encoder0.getPulses() - initial0) <= (oneCellCountMomentum-200) && (encoder1.getPulses() - initial1) <= (oneCellCountMomentum-200)) { |
christine222 | 23:690b0ca34ee9 | 349 | //while( (IRP_1.getSamples(50) + IRP_4.getSamples(50))/2 < ((IRP_1.sensorAvg+IRP_2.sensorAvg)/2)*0.4 ){ |
christine222 | 21:9a6cb07bdcb6 | 350 | //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 )); |
christine222 | 21:9a6cb07bdcb6 | 351 | |
christine222 | 21:9a6cb07bdcb6 | 352 | count0 = encoder0.getPulses() - initial0; |
christine222 | 21:9a6cb07bdcb6 | 353 | count1 = encoder1.getPulses() - initial1; |
christine222 | 21:9a6cb07bdcb6 | 354 | int x = count0 - count1; |
christine222 | 21:9a6cb07bdcb6 | 355 | //double d = kp * x + kd * ( x - prev ); |
christine222 | 21:9a6cb07bdcb6 | 356 | double kppart = kp * x; |
christine222 | 21:9a6cb07bdcb6 | 357 | double kdpart = kd * (x-prev); |
christine222 | 21:9a6cb07bdcb6 | 358 | double d = kppart + kdpart; |
christine222 | 21:9a6cb07bdcb6 | 359 | |
christine222 | 21:9a6cb07bdcb6 | 360 | //serial.printf( "x: %d,\t prev: %d,\t d: %f,\t kppart: %f,\t kdpart: %f\n", x, prev, d, kppart, kdpart ); |
christine222 | 21:9a6cb07bdcb6 | 361 | if( x < diff - 40 ) // count1 is bigger, right wheel pushed forward |
christine222 | 21:9a6cb07bdcb6 | 362 | { |
christine222 | 21:9a6cb07bdcb6 | 363 | left_motor.move( speed1-0.8*d ); |
christine222 | 21:9a6cb07bdcb6 | 364 | right_motor.move( speed0+d ); |
christine222 | 21:9a6cb07bdcb6 | 365 | } |
christine222 | 21:9a6cb07bdcb6 | 366 | else if( x > diff + 40 ) |
christine222 | 21:9a6cb07bdcb6 | 367 | { |
christine222 | 21:9a6cb07bdcb6 | 368 | left_motor.move( speed1-0.8*d ); |
christine222 | 21:9a6cb07bdcb6 | 369 | right_motor.move( speed0+d ); |
christine222 | 21:9a6cb07bdcb6 | 370 | } |
christine222 | 21:9a6cb07bdcb6 | 371 | // else |
christine222 | 21:9a6cb07bdcb6 | 372 | // { |
christine222 | 21:9a6cb07bdcb6 | 373 | // left_motor.brake(); |
christine222 | 21:9a6cb07bdcb6 | 374 | // right_motor.brake(); |
christine222 | 21:9a6cb07bdcb6 | 375 | // } |
christine222 | 21:9a6cb07bdcb6 | 376 | prev = x; |
christine222 | 21:9a6cb07bdcb6 | 377 | } |
christine222 | 25:f827a8b7880e | 378 | |
christine222 | 21:9a6cb07bdcb6 | 379 | //pidOnEncoders(); |
christine222 | 23:690b0ca34ee9 | 380 | //pidBrake(); |
christine222 | 23:690b0ca34ee9 | 381 | right_motor.brake(); |
christine222 | 23:690b0ca34ee9 | 382 | left_motor.brake(); |
christine222 | 23:690b0ca34ee9 | 383 | return; |
christine222 | 23:690b0ca34ee9 | 384 | } |
christine222 | 25:f827a8b7880e | 385 | |
christine222 | 25:f827a8b7880e | 386 | |
christine222 | 23:690b0ca34ee9 | 387 | void moveForwardWallEncoder(){ |
christine222 | 25:f827a8b7880e | 388 | |
christine222 | 23:690b0ca34ee9 | 389 | int count0; |
christine222 | 23:690b0ca34ee9 | 390 | int count1; |
christine222 | 23:690b0ca34ee9 | 391 | count0 = encoder0.getPulses(); |
christine222 | 23:690b0ca34ee9 | 392 | count1 = encoder1.getPulses(); |
christine222 | 23:690b0ca34ee9 | 393 | int initial1 = count1; |
christine222 | 23:690b0ca34ee9 | 394 | int initial0 = count0; |
christine222 | 23:690b0ca34ee9 | 395 | int diff = count0 - count1; |
christine222 | 23:690b0ca34ee9 | 396 | double kp = 0.00015; |
christine222 | 23:690b0ca34ee9 | 397 | double kd = 0.00019; |
christine222 | 23:690b0ca34ee9 | 398 | int prev = 0; |
christine222 | 25:f827a8b7880e | 399 | |
christine222 | 25:f827a8b7880e | 400 | |
christine222 | 25:f827a8b7880e | 401 | |
christine222 | 25:f827a8b7880e | 402 | double speed0 = 0.11; |
christine222 | 25:f827a8b7880e | 403 | double speed1 = 0.13; |
christine222 | 23:690b0ca34ee9 | 404 | right_motor.move(speed0); |
christine222 | 23:690b0ca34ee9 | 405 | left_motor.move(speed1); |
christine222 | 23:690b0ca34ee9 | 406 | |
christine222 | 25:f827a8b7880e | 407 | float ir1 = IRP_1.getSamples(50); |
christine222 | 25:f827a8b7880e | 408 | float ir4 = IRP_4.getSamples(50); |
christine222 | 25:f827a8b7880e | 409 | |
christine222 | 25:f827a8b7880e | 410 | if((ir1 + ir4)/2 > ((IRP_1.sensorAvg+IRP_4.sensorAvg)/2)*0.4){ |
christine222 | 24:e7063765d6f0 | 411 | return; |
christine222 | 24:e7063765d6f0 | 412 | } |
christine222 | 25:f827a8b7880e | 413 | |
christine222 | 23:690b0ca34ee9 | 414 | //while((encoder0.getPulses() - initial0) <= (oneCellCountMomentum-200) && (encoder1.getPulses() - initial1) <= (oneCellCountMomentum-200)) { |
christine222 | 25:f827a8b7880e | 415 | //while( (ir1 + ir4)/2 < ((IRP_1.sensorAvg+IRP_4.sensorAvg)/2)*0.4 ){ |
christine222 | 25:f827a8b7880e | 416 | while( ir1 < IRP_1.sensorAvg*0.7 || ir4 < IRP_4.sensorAvg*0.7 ){ |
christine222 | 23:690b0ca34ee9 | 417 | //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 )); |
christine222 | 23:690b0ca34ee9 | 418 | |
christine222 | 23:690b0ca34ee9 | 419 | count0 = encoder0.getPulses() - initial0; |
christine222 | 23:690b0ca34ee9 | 420 | count1 = encoder1.getPulses() - initial1; |
christine222 | 23:690b0ca34ee9 | 421 | int x = count0 - count1; |
christine222 | 23:690b0ca34ee9 | 422 | //double d = kp * x + kd * ( x - prev ); |
christine222 | 23:690b0ca34ee9 | 423 | double kppart = kp * x; |
christine222 | 23:690b0ca34ee9 | 424 | double kdpart = kd * (x-prev); |
christine222 | 23:690b0ca34ee9 | 425 | double d = kppart + kdpart; |
christine222 | 23:690b0ca34ee9 | 426 | |
christine222 | 23:690b0ca34ee9 | 427 | //serial.printf( "x: %d,\t prev: %d,\t d: %f,\t kppart: %f,\t kdpart: %f\n", x, prev, d, kppart, kdpart ); |
christine222 | 23:690b0ca34ee9 | 428 | if( x < diff - 40 ) // count1 is bigger, right wheel pushed forward |
christine222 | 23:690b0ca34ee9 | 429 | { |
christine222 | 23:690b0ca34ee9 | 430 | left_motor.move( speed1-0.8*d ); |
christine222 | 23:690b0ca34ee9 | 431 | right_motor.move( speed0+d ); |
christine222 | 23:690b0ca34ee9 | 432 | } |
christine222 | 23:690b0ca34ee9 | 433 | else if( x > diff + 40 ) |
christine222 | 23:690b0ca34ee9 | 434 | { |
christine222 | 23:690b0ca34ee9 | 435 | left_motor.move( speed1-0.8*d ); |
christine222 | 23:690b0ca34ee9 | 436 | right_motor.move( speed0+d ); |
christine222 | 23:690b0ca34ee9 | 437 | } |
christine222 | 23:690b0ca34ee9 | 438 | // else |
christine222 | 23:690b0ca34ee9 | 439 | // { |
christine222 | 23:690b0ca34ee9 | 440 | // left_motor.brake(); |
christine222 | 23:690b0ca34ee9 | 441 | // right_motor.brake(); |
christine222 | 23:690b0ca34ee9 | 442 | // } |
christine222 | 23:690b0ca34ee9 | 443 | prev = x; |
christine222 | 25:f827a8b7880e | 444 | ir1 = IRP_1.getSamples(50); |
christine222 | 25:f827a8b7880e | 445 | ir4 = IRP_4.getSamples(50); |
christine222 | 23:690b0ca34ee9 | 446 | } |
christine222 | 25:f827a8b7880e | 447 | |
christine222 | 23:690b0ca34ee9 | 448 | //pidOnEncoders(); |
christine222 | 23:690b0ca34ee9 | 449 | //pidBrake(); |
christine222 | 23:690b0ca34ee9 | 450 | right_motor.brake(); |
christine222 | 23:690b0ca34ee9 | 451 | left_motor.brake(); |
christine222 | 21:9a6cb07bdcb6 | 452 | return; |
christine222 | 21:9a6cb07bdcb6 | 453 | } |
christine222 | 25:f827a8b7880e | 454 | |
sahilmgandhi | 14:9e7bb03ddccb | 455 | void moveForwardUntilWallIr() |
sahilmgandhi | 14:9e7bb03ddccb | 456 | { |
kyleliangus | 9:1d8e4da058cd | 457 | double currentError = 0; |
kyleliangus | 9:1d8e4da058cd | 458 | double previousError = 0; |
kyleliangus | 9:1d8e4da058cd | 459 | double derivError = 0; |
kyleliangus | 9:1d8e4da058cd | 460 | double sumError = 0; |
christine222 | 25:f827a8b7880e | 461 | |
vanshg | 11:8fc2b703086b | 462 | double HIGH_PWM_VOLTAGE = 0.1; |
christine222 | 25:f827a8b7880e | 463 | |
christine222 | 25:f827a8b7880e | 464 | double rightSpeed = 0.25; |
christine222 | 25:f827a8b7880e | 465 | double leftSpeed = 0.23; |
christine222 | 25:f827a8b7880e | 466 | |
kyleliangus | 9:1d8e4da058cd | 467 | float ir2 = IRP_2.getSamples( SAMPLE_NUM ); |
kyleliangus | 9:1d8e4da058cd | 468 | float ir3 = IRP_3.getSamples( SAMPLE_NUM ); |
christine222 | 25:f827a8b7880e | 469 | |
vanshg | 11:8fc2b703086b | 470 | int count = encoder0.getPulses(); |
christine222 | 21:9a6cb07bdcb6 | 471 | while ((IRP_1.getSamples( SAMPLE_NUM ) + IRP_4.getSamples( SAMPLE_NUM ) )/2 < 0.05f) { // while the front facing IR's arent covered |
christine222 | 21:9a6cb07bdcb6 | 472 | |
christine222 | 21:9a6cb07bdcb6 | 473 | if((IRP_2.getSamples(SAMPLE_NUM) < 0.005 || IRP_3.getSamples(SAMPLE_NUM) < 0.005)) { |
christine222 | 23:690b0ca34ee9 | 474 | //moveForwardWallEncoder(); |
christine222 | 22:681190ff98f0 | 475 | }else if(IRP_2.getSamples(SAMPLE_NUM) < 0.005){ // left wall gone |
christine222 | 22:681190ff98f0 | 476 | //moveForwardRightWall(); |
christine222 | 22:681190ff98f0 | 477 | }else if(IRP_3.getSamples(SAMPLE_NUM) < 0.005){ // right wall gone |
christine222 | 22:681190ff98f0 | 478 | //moveForwardLeftWall(); |
christine222 | 21:9a6cb07bdcb6 | 479 | }else{ |
christine222 | 21:9a6cb07bdcb6 | 480 | // will move forward using encoders only |
christine222 | 21:9a6cb07bdcb6 | 481 | // if cell ahead doesn't have a wall on either one side or both sides |
christine222 | 25:f827a8b7880e | 482 | |
christine222 | 21:9a6cb07bdcb6 | 483 | int pulseCount = (encoder0.getPulses()-count) % 5600; |
christine222 | 21:9a6cb07bdcb6 | 484 | if (pulseCount > 5400 && pulseCount < 5800) { |
christine222 | 21:9a6cb07bdcb6 | 485 | blueLed.write(0); |
christine222 | 21:9a6cb07bdcb6 | 486 | } else { |
christine222 | 21:9a6cb07bdcb6 | 487 | blueLed.write(1); |
christine222 | 21:9a6cb07bdcb6 | 488 | } |
christine222 | 21:9a6cb07bdcb6 | 489 | sumError += currentError; |
christine222 | 25:f827a8b7880e | 490 | currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg/initAverageL) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg/initAverageR) ) ; |
christine222 | 21:9a6cb07bdcb6 | 491 | derivError = currentError - previousError; |
christine222 | 21:9a6cb07bdcb6 | 492 | double PIDSum = IP_CONSTANT*currentError + II_CONSTANT*sumError + ID_CONSTANT*derivError; |
christine222 | 21:9a6cb07bdcb6 | 493 | if (PIDSum > 0) { // this means the leftWheel is faster than the right. So right speeds up, left slows down |
christine222 | 21:9a6cb07bdcb6 | 494 | rightSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE); |
christine222 | 21:9a6cb07bdcb6 | 495 | leftSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE); |
christine222 | 21:9a6cb07bdcb6 | 496 | } else { // r is faster than L. speed up l, slow down r |
christine222 | 21:9a6cb07bdcb6 | 497 | rightSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE); |
christine222 | 21:9a6cb07bdcb6 | 498 | leftSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE); |
christine222 | 21:9a6cb07bdcb6 | 499 | } |
christine222 | 25:f827a8b7880e | 500 | |
christine222 | 21:9a6cb07bdcb6 | 501 | if (leftSpeed > 0.30) leftSpeed = 0.30; |
christine222 | 21:9a6cb07bdcb6 | 502 | if (leftSpeed < 0) leftSpeed = 0; |
christine222 | 21:9a6cb07bdcb6 | 503 | if (rightSpeed > 0.30) rightSpeed = 0.30; |
christine222 | 21:9a6cb07bdcb6 | 504 | if (rightSpeed < 0) rightSpeed = 0; |
christine222 | 25:f827a8b7880e | 505 | |
christine222 | 21:9a6cb07bdcb6 | 506 | right_motor.forward(rightSpeed); |
christine222 | 21:9a6cb07bdcb6 | 507 | left_motor.forward(leftSpeed); |
christine222 | 25:f827a8b7880e | 508 | |
christine222 | 21:9a6cb07bdcb6 | 509 | previousError = currentError; |
christine222 | 25:f827a8b7880e | 510 | |
christine222 | 21:9a6cb07bdcb6 | 511 | ir2 = IRP_2.getSamples( SAMPLE_NUM ); |
christine222 | 21:9a6cb07bdcb6 | 512 | ir3 = IRP_3.getSamples( SAMPLE_NUM ); |
christine222 | 25:f827a8b7880e | 513 | |
kyleliangus | 9:1d8e4da058cd | 514 | } |
christine222 | 25:f827a8b7880e | 515 | |
christine222 | 21:9a6cb07bdcb6 | 516 | //backward(); |
christine222 | 22:681190ff98f0 | 517 | //wait_ms(40); |
christine222 | 21:9a6cb07bdcb6 | 518 | //brake(); |
christine222 | 25:f827a8b7880e | 519 | |
christine222 | 21:9a6cb07bdcb6 | 520 | left_motor.brake(); |
christine222 | 21:9a6cb07bdcb6 | 521 | right_motor.brake(); |
kyleliangus | 9:1d8e4da058cd | 522 | } |
vanshg | 10:810d1849da9d | 523 | } |
christine222 | 25:f827a8b7880e | 524 | |
sahilmgandhi | 14:9e7bb03ddccb | 525 | void printDipFlag() |
sahilmgandhi | 14:9e7bb03ddccb | 526 | { |
vanshg | 11:8fc2b703086b | 527 | if (DEBUGGING) serial.printf("Flag value is %d", dipFlags); |
vanshg | 11:8fc2b703086b | 528 | } |
christine222 | 25:f827a8b7880e | 529 | |
sahilmgandhi | 14:9e7bb03ddccb | 530 | void enableButton1() |
sahilmgandhi | 14:9e7bb03ddccb | 531 | { |
vanshg | 10:810d1849da9d | 532 | dipFlags |= BUTTON1_FLAG; |
vanshg | 11:8fc2b703086b | 533 | printDipFlag(); |
vanshg | 10:810d1849da9d | 534 | } |
sahilmgandhi | 14:9e7bb03ddccb | 535 | void enableButton2() |
sahilmgandhi | 14:9e7bb03ddccb | 536 | { |
vanshg | 10:810d1849da9d | 537 | dipFlags |= BUTTON2_FLAG; |
vanshg | 11:8fc2b703086b | 538 | printDipFlag(); |
vanshg | 10:810d1849da9d | 539 | } |
sahilmgandhi | 14:9e7bb03ddccb | 540 | void enableButton3() |
sahilmgandhi | 14:9e7bb03ddccb | 541 | { |
vanshg | 10:810d1849da9d | 542 | dipFlags |= BUTTON3_FLAG; |
vanshg | 11:8fc2b703086b | 543 | printDipFlag(); |
vanshg | 10:810d1849da9d | 544 | } |
sahilmgandhi | 14:9e7bb03ddccb | 545 | void enableButton4() |
sahilmgandhi | 14:9e7bb03ddccb | 546 | { |
vanshg | 10:810d1849da9d | 547 | dipFlags |= BUTTON4_FLAG; |
vanshg | 11:8fc2b703086b | 548 | printDipFlag(); |
vanshg | 10:810d1849da9d | 549 | } |
sahilmgandhi | 14:9e7bb03ddccb | 550 | void disableButton1() |
sahilmgandhi | 14:9e7bb03ddccb | 551 | { |
vanshg | 10:810d1849da9d | 552 | dipFlags &= ~BUTTON1_FLAG; |
vanshg | 11:8fc2b703086b | 553 | printDipFlag(); |
vanshg | 10:810d1849da9d | 554 | } |
sahilmgandhi | 14:9e7bb03ddccb | 555 | void disableButton2() |
sahilmgandhi | 14:9e7bb03ddccb | 556 | { |
vanshg | 10:810d1849da9d | 557 | dipFlags &= ~BUTTON2_FLAG; |
vanshg | 11:8fc2b703086b | 558 | printDipFlag(); |
vanshg | 10:810d1849da9d | 559 | } |
sahilmgandhi | 14:9e7bb03ddccb | 560 | void disableButton3() |
sahilmgandhi | 14:9e7bb03ddccb | 561 | { |
vanshg | 10:810d1849da9d | 562 | dipFlags &= ~BUTTON3_FLAG; |
vanshg | 11:8fc2b703086b | 563 | printDipFlag(); |
vanshg | 10:810d1849da9d | 564 | } |
sahilmgandhi | 14:9e7bb03ddccb | 565 | void disableButton4() |
sahilmgandhi | 14:9e7bb03ddccb | 566 | { |
vanshg | 10:810d1849da9d | 567 | dipFlags &= ~BUTTON4_FLAG; |
vanshg | 11:8fc2b703086b | 568 | printDipFlag(); |
kyleliangus | 9:1d8e4da058cd | 569 | } |
christine222 | 25:f827a8b7880e | 570 | |
kyleliangus | 15:b80555a4a8b9 | 571 | void pidOnEncoders() |
kyleliangus | 15:b80555a4a8b9 | 572 | { |
kyleliangus | 15:b80555a4a8b9 | 573 | int count0; |
kyleliangus | 15:b80555a4a8b9 | 574 | int count1; |
kyleliangus | 15:b80555a4a8b9 | 575 | count0 = encoder0.getPulses(); |
kyleliangus | 15:b80555a4a8b9 | 576 | count1 = encoder1.getPulses(); |
kyleliangus | 15:b80555a4a8b9 | 577 | int diff = count0 - count1; |
sahilmgandhi | 20:82836745332e | 578 | double kp = 0.00011; |
sahilmgandhi | 20:82836745332e | 579 | double kd = 0.00014; |
kyleliangus | 15:b80555a4a8b9 | 580 | int prev = 0; |
christine222 | 25:f827a8b7880e | 581 | |
sahilmgandhi | 16:d9252437bd92 | 582 | int counter = 0; |
kyleliangus | 15:b80555a4a8b9 | 583 | while(1) |
kyleliangus | 15:b80555a4a8b9 | 584 | { |
kyleliangus | 15:b80555a4a8b9 | 585 | count0 = encoder0.getPulses(); |
kyleliangus | 15:b80555a4a8b9 | 586 | count1 = encoder1.getPulses(); |
kyleliangus | 15:b80555a4a8b9 | 587 | int x = count0 - count1; |
kyleliangus | 15:b80555a4a8b9 | 588 | //double d = kp * x + kd * ( x - prev ); |
kyleliangus | 15:b80555a4a8b9 | 589 | double kppart = kp * x; |
kyleliangus | 15:b80555a4a8b9 | 590 | double kdpart = kd * (x-prev); |
kyleliangus | 15:b80555a4a8b9 | 591 | double d = kppart + kdpart; |
kyleliangus | 15:b80555a4a8b9 | 592 | |
kyleliangus | 15:b80555a4a8b9 | 593 | //serial.printf( "x: %d,\t prev: %d,\t d: %f,\t kppart: %f,\t kdpart: %f\n", x, prev, d, kppart, kdpart ); |
sahilmgandhi | 20:82836745332e | 594 | if( x < diff - 40 ) // count1 is bigger, right wheel pushed forward |
kyleliangus | 15:b80555a4a8b9 | 595 | { |
kyleliangus | 15:b80555a4a8b9 | 596 | left_motor.move( -d ); |
kyleliangus | 15:b80555a4a8b9 | 597 | right_motor.move( d ); |
kyleliangus | 15:b80555a4a8b9 | 598 | } |
sahilmgandhi | 20:82836745332e | 599 | else if( x > diff + 40 ) |
kyleliangus | 15:b80555a4a8b9 | 600 | { |
kyleliangus | 15:b80555a4a8b9 | 601 | left_motor.move( -d ); |
kyleliangus | 15:b80555a4a8b9 | 602 | right_motor.move( d ); |
kyleliangus | 15:b80555a4a8b9 | 603 | } |
sahilmgandhi | 16:d9252437bd92 | 604 | // else |
sahilmgandhi | 16:d9252437bd92 | 605 | // { |
sahilmgandhi | 16:d9252437bd92 | 606 | // left_motor.brake(); |
sahilmgandhi | 16:d9252437bd92 | 607 | // right_motor.brake(); |
sahilmgandhi | 16:d9252437bd92 | 608 | // } |
kyleliangus | 15:b80555a4a8b9 | 609 | prev = x; |
sahilmgandhi | 16:d9252437bd92 | 610 | counter++; |
sahilmgandhi | 19:7b66a518b6f8 | 611 | if (counter == 5) |
sahilmgandhi | 16:d9252437bd92 | 612 | break; |
kyleliangus | 15:b80555a4a8b9 | 613 | } |
kyleliangus | 15:b80555a4a8b9 | 614 | } |
christine222 | 25:f827a8b7880e | 615 | |
christine222 | 22:681190ff98f0 | 616 | void nCellEncoderAndIR(double cellCount){ |
sahilmgandhi | 19:7b66a518b6f8 | 617 | double currentError = 0; |
sahilmgandhi | 19:7b66a518b6f8 | 618 | double previousError = 0; |
sahilmgandhi | 19:7b66a518b6f8 | 619 | double derivError = 0; |
sahilmgandhi | 19:7b66a518b6f8 | 620 | double sumError = 0; |
christine222 | 25:f827a8b7880e | 621 | |
christine222 | 25:f827a8b7880e | 622 | double HIGH_PWM_VOLTAGE = 0.1; |
christine222 | 25:f827a8b7880e | 623 | double rightSpeed = 0.10; |
christine222 | 25:f827a8b7880e | 624 | double leftSpeed = 0.10; |
christine222 | 25:f827a8b7880e | 625 | |
christine222 | 25:f827a8b7880e | 626 | int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellCount; |
christine222 | 25:f827a8b7880e | 627 | int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellCount; |
christine222 | 25:f827a8b7880e | 628 | |
christine222 | 25:f827a8b7880e | 629 | left_motor.forward(0.28); |
christine222 | 25:f827a8b7880e | 630 | right_motor.forward(0.25); |
christine222 | 25:f827a8b7880e | 631 | |
christine222 | 25:f827a8b7880e | 632 | float receiverTwoReading = 0.0; |
christine222 | 25:f827a8b7880e | 633 | float receiverThreeReading = 0.0; |
christine222 | 25:f827a8b7880e | 634 | |
sahilmgandhi | 19:7b66a518b6f8 | 635 | float ir2 = IRP_2.getSamples( SAMPLE_NUM ); |
sahilmgandhi | 19:7b66a518b6f8 | 636 | float ir3 = IRP_3.getSamples( SAMPLE_NUM ); |
christine222 | 25:f827a8b7880e | 637 | |
christine222 | 25:f827a8b7880e | 638 | // float previr2 = ir2; |
christine222 | 25:f827a8b7880e | 639 | // float previr3 = ir3; |
christine222 | 25:f827a8b7880e | 640 | |
christine222 | 23:690b0ca34ee9 | 641 | int state = 0; |
christine222 | 23:690b0ca34ee9 | 642 | |
christine222 | 25:f827a8b7880e | 643 | |
christine222 | 25:f827a8b7880e | 644 | while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1 && receiverOneReading < IRP_1.sensorAvg*0.8 && receiverFourReading < IRP_4.sensorAvg*0.8){ |
christine222 | 25:f827a8b7880e | 645 | receiverTwoReading = IRP_2.getSamples(100); |
christine222 | 25:f827a8b7880e | 646 | receiverThreeReading = IRP_3.getSamples(100); |
christine222 | 25:f827a8b7880e | 647 | // previr2 = receiverTwoReading; |
christine222 | 25:f827a8b7880e | 648 | // previr3 = receiverThreeReading; |
christine222 | 25:f827a8b7880e | 649 | receiverOneReading = IRP_1.getSamples(100); |
christine222 | 25:f827a8b7880e | 650 | receiverFourReading = IRP_4.getSamples(100); |
christine222 | 25:f827a8b7880e | 651 | |
christine222 | 25:f827a8b7880e | 652 | //if ((receiverOneReading+receiverFourReading)/2 > ((IRP_1.sensorAvg+IRP_4.sensorAvg)/2)*0.15 ){ |
christine222 | 25:f827a8b7880e | 653 | if( receiverOneReading > IRP_1.sensorAvg*0.7 || receiverFourReading > IRP_4.sensorAvg*0.7 ){ |
christine222 | 25:f827a8b7880e | 654 | // almost to the end |
christine222 | 25:f827a8b7880e | 655 | right_motor.move(-0.15); |
christine222 | 25:f827a8b7880e | 656 | left_motor.move(-0.15); |
christine222 | 25:f827a8b7880e | 657 | |
christine222 | 25:f827a8b7880e | 658 | wait_ms(150); |
christine222 | 25:f827a8b7880e | 659 | right_motor.brake(); |
christine222 | 25:f827a8b7880e | 660 | left_motor.brake(); |
christine222 | 25:f827a8b7880e | 661 | |
christine222 | 25:f827a8b7880e | 662 | |
christine222 | 25:f827a8b7880e | 663 | redLed.write(1); |
christine222 | 25:f827a8b7880e | 664 | greenLed.write(1); |
christine222 | 25:f827a8b7880e | 665 | blueLed.write(1); |
christine222 | 25:f827a8b7880e | 666 | wait_ms(200); |
christine222 | 25:f827a8b7880e | 667 | redLed.write(1); |
christine222 | 25:f827a8b7880e | 668 | greenLed.write(1); |
christine222 | 25:f827a8b7880e | 669 | blueLed.write(0); |
christine222 | 25:f827a8b7880e | 670 | wait_ms(200); |
christine222 | 25:f827a8b7880e | 671 | |
christine222 | 25:f827a8b7880e | 672 | |
christine222 | 25:f827a8b7880e | 673 | |
christine222 | 25:f827a8b7880e | 674 | redLed.write(1); |
christine222 | 25:f827a8b7880e | 675 | greenLed.write(0); |
christine222 | 25:f827a8b7880e | 676 | blueLed.write(1); |
christine222 | 25:f827a8b7880e | 677 | wait_ms(200); |
christine222 | 25:f827a8b7880e | 678 | redLed.write(0); |
christine222 | 25:f827a8b7880e | 679 | greenLed.write(1); |
christine222 | 25:f827a8b7880e | 680 | blueLed.write(1); |
christine222 | 25:f827a8b7880e | 681 | |
christine222 | 25:f827a8b7880e | 682 | |
christine222 | 25:f827a8b7880e | 683 | |
christine222 | 25:f827a8b7880e | 684 | |
christine222 | 25:f827a8b7880e | 685 | //moveForwardWallEncoder(); |
christine222 | 25:f827a8b7880e | 686 | |
christine222 | 25:f827a8b7880e | 687 | |
christine222 | 25:f827a8b7880e | 688 | return; |
christine222 | 25:f827a8b7880e | 689 | |
christine222 | 25:f827a8b7880e | 690 | }else if((receiverThreeReading < 1.3*IRP_3.sensorAvg/(averageDivR)) && (receiverTwoReading < 1.3*IRP_2.sensorAvg/(averageDivL)) ){ |
christine222 | 25:f827a8b7880e | 691 | // both sides gone |
christine222 | 25:f827a8b7880e | 692 | redLed.write(1); |
christine222 | 25:f827a8b7880e | 693 | greenLed.write(1); |
christine222 | 25:f827a8b7880e | 694 | blueLed.write(1); |
christine222 | 25:f827a8b7880e | 695 | wait_ms(100); |
christine222 | 25:f827a8b7880e | 696 | redLed.write(1); |
christine222 | 25:f827a8b7880e | 697 | greenLed.write(1); |
christine222 | 25:f827a8b7880e | 698 | blueLed.write(0); |
christine222 | 25:f827a8b7880e | 699 | wait_ms(200); |
christine222 | 25:f827a8b7880e | 700 | redLed.write(1); |
christine222 | 25:f827a8b7880e | 701 | greenLed.write(1); |
christine222 | 25:f827a8b7880e | 702 | blueLed.write(0); |
christine222 | 25:f827a8b7880e | 703 | wait_ms(200); |
christine222 | 25:f827a8b7880e | 704 | redLed.write(1); |
christine222 | 25:f827a8b7880e | 705 | greenLed.write(1); |
christine222 | 25:f827a8b7880e | 706 | blueLed.write(0); |
christine222 | 25:f827a8b7880e | 707 | wait_ms(200); |
christine222 | 25:f827a8b7880e | 708 | redLed.write(1); |
christine222 | 25:f827a8b7880e | 709 | greenLed.write(1); |
christine222 | 25:f827a8b7880e | 710 | blueLed.write(0); |
christine222 | 25:f827a8b7880e | 711 | moveForwardEncoder(); |
christine222 | 25:f827a8b7880e | 712 | }else if (receiverThreeReading < IRP_3.sensorAvg/averageDivR){// right wall gone |
christine222 | 25:f827a8b7880e | 713 | // RED RED RED RED RED |
christine222 | 25:f827a8b7880e | 714 | state = 1; |
christine222 | 25:f827a8b7880e | 715 | redLed.write(0); |
christine222 | 25:f827a8b7880e | 716 | greenLed.write(1); |
christine222 | 25:f827a8b7880e | 717 | blueLed.write(1); |
christine222 | 25:f827a8b7880e | 718 | }else if (receiverTwoReading < IRP_2.sensorAvg/averageDivL){// left wall gone |
christine222 | 25:f827a8b7880e | 719 | // BLUE BLUE BLUE BLUE |
christine222 | 25:f827a8b7880e | 720 | state = 2; |
christine222 | 25:f827a8b7880e | 721 | redLed.write(1); |
christine222 | 25:f827a8b7880e | 722 | greenLed.write(1); |
christine222 | 25:f827a8b7880e | 723 | blueLed.write(0); |
christine222 | 25:f827a8b7880e | 724 | }else if ((receiverTwoReading > ((IRP_2.sensorAvg/initAverageL)*averageDivUpper)) && (receiverThreeReading > ((IRP_3.sensorAvg/initAverageR)*averageDivUpper))){ |
christine222 | 25:f827a8b7880e | 725 | // both walls there |
christine222 | 25:f827a8b7880e | 726 | state = 0; |
christine222 | 25:f827a8b7880e | 727 | redLed.write(1); |
christine222 | 25:f827a8b7880e | 728 | greenLed.write(0); |
christine222 | 25:f827a8b7880e | 729 | blueLed.write(1); |
christine222 | 25:f827a8b7880e | 730 | } |
christine222 | 25:f827a8b7880e | 731 | |
christine222 | 25:f827a8b7880e | 732 | switch(state){ |
christine222 | 25:f827a8b7880e | 733 | case(0):{ // both walls there |
christine222 | 25:f827a8b7880e | 734 | currentError = ( receiverTwoReading - IRP_2.sensorAvg/initAverageL) - ( receiverThreeReading - IRP_3.sensorAvg/initAverageR); |
christine222 | 25:f827a8b7880e | 735 | break; |
christine222 | 25:f827a8b7880e | 736 | } |
christine222 | 25:f827a8b7880e | 737 | case(1):{// RED RED RED RED RED |
christine222 | 25:f827a8b7880e | 738 | currentError = (receiverTwoReading - IRP_2.sensorAvg/initAverageL) - (IRP_2.sensorAvg/initAverageL); |
christine222 | 25:f827a8b7880e | 739 | break; |
christine222 | 25:f827a8b7880e | 740 | } |
christine222 | 25:f827a8b7880e | 741 | case(2):{// blue |
christine222 | 25:f827a8b7880e | 742 | currentError = (IRP_3.sensorAvg/initAverageR) - (receiverThreeReading - IRP_3.sensorAvg/initAverageR); |
christine222 | 25:f827a8b7880e | 743 | break; |
christine222 | 25:f827a8b7880e | 744 | } |
christine222 | 25:f827a8b7880e | 745 | default:{ |
christine222 | 25:f827a8b7880e | 746 | currentError = ( receiverTwoReading - IRP_2.sensorAvg/initAverageL) - ( receiverThreeReading - IRP_3.sensorAvg/initAverageR); |
christine222 | 25:f827a8b7880e | 747 | //currentError = ( receiverTwoReading - IRP_2.sensorAvg/initAverageL) - ( receiverThreeReading - IRP_3.sensorAvg/initAverageR); |
christine222 | 25:f827a8b7880e | 748 | break; |
christine222 | 25:f827a8b7880e | 749 | } |
christine222 | 25:f827a8b7880e | 750 | } |
christine222 | 25:f827a8b7880e | 751 | //currentError = ( receiverTwoReading - IRP_2.sensorAvg/initAverageL) - ( receiverThreeReading - IRP_3.sensorAvg/initAverageR); |
christine222 | 25:f827a8b7880e | 752 | |
christine222 | 25:f827a8b7880e | 753 | |
christine222 | 25:f827a8b7880e | 754 | |
christine222 | 25:f827a8b7880e | 755 | |
christine222 | 25:f827a8b7880e | 756 | |
christine222 | 25:f827a8b7880e | 757 | sumError += currentError; |
christine222 | 25:f827a8b7880e | 758 | derivError = currentError - previousError; |
christine222 | 25:f827a8b7880e | 759 | double PIDSum = IP_CONSTANT*currentError + II_CONSTANT*sumError + ID_CONSTANT*derivError; |
christine222 | 25:f827a8b7880e | 760 | if (PIDSum > 0) { // this means the leftWheel is faster than the right. So right speeds up, left slows down |
christine222 | 25:f827a8b7880e | 761 | rightSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE); |
christine222 | 25:f827a8b7880e | 762 | leftSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE); |
christine222 | 25:f827a8b7880e | 763 | } else { // r is faster than L. speed up l, slow down r |
christine222 | 25:f827a8b7880e | 764 | rightSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE); |
christine222 | 25:f827a8b7880e | 765 | leftSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE); |
christine222 | 25:f827a8b7880e | 766 | } |
christine222 | 25:f827a8b7880e | 767 | if (leftSpeed > 0.30) leftSpeed = 0.30; |
christine222 | 25:f827a8b7880e | 768 | if (leftSpeed < 0) leftSpeed = 0; |
christine222 | 25:f827a8b7880e | 769 | if (rightSpeed > 0.30) rightSpeed = 0.30; |
christine222 | 25:f827a8b7880e | 770 | if (rightSpeed < 0) rightSpeed = 0; |
christine222 | 25:f827a8b7880e | 771 | |
christine222 | 25:f827a8b7880e | 772 | right_motor.forward(rightSpeed); |
christine222 | 25:f827a8b7880e | 773 | left_motor.forward(leftSpeed); |
christine222 | 25:f827a8b7880e | 774 | pidOnEncoders(); |
christine222 | 25:f827a8b7880e | 775 | |
christine222 | 25:f827a8b7880e | 776 | previousError = currentError; |
christine222 | 25:f827a8b7880e | 777 | ir2 = IRP_2.getSamples( SAMPLE_NUM ); |
christine222 | 25:f827a8b7880e | 778 | ir3 = IRP_3.getSamples( SAMPLE_NUM ); |
christine222 | 25:f827a8b7880e | 779 | |
sahilmgandhi | 19:7b66a518b6f8 | 780 | } |
sahilmgandhi | 19:7b66a518b6f8 | 781 | |
christine222 | 21:9a6cb07bdcb6 | 782 | |
christine222 | 21:9a6cb07bdcb6 | 783 | |
sahilmgandhi | 19:7b66a518b6f8 | 784 | left_motor.brake(); |
sahilmgandhi | 19:7b66a518b6f8 | 785 | right_motor.brake(); |
christine222 | 23:690b0ca34ee9 | 786 | return; |
sahilmgandhi | 17:f713758f6238 | 787 | } |
christine222 | 25:f827a8b7880e | 788 | |
sahilmgandhi | 0:a03c771ab78e | 789 | int main() |
sahilmgandhi | 0:a03c771ab78e | 790 | { |
christine222 | 3:880f15be8c72 | 791 | //Set highest bandwidth. |
christine222 | 23:690b0ca34ee9 | 792 | //gyro.setLpBandwidth(LPFBW_42HZ); |
christine222 | 3:880f15be8c72 | 793 | serial.baud(9600); |
christine222 | 23:690b0ca34ee9 | 794 | //serial.printf("The gyro's address is %s", gyro.getWhoAmI()); |
christine222 | 25:f827a8b7880e | 795 | |
sahilmgandhi | 1:8a4b2f573923 | 796 | wait (0.1); |
christine222 | 25:f827a8b7880e | 797 | |
christine222 | 25:f827a8b7880e | 798 | |
sahilmgandhi | 2:771db996cee0 | 799 | redLed.write(1); |
sahilmgandhi | 14:9e7bb03ddccb | 800 | greenLed.write(0); |
sahilmgandhi | 2:771db996cee0 | 801 | blueLed.write(1); |
christine222 | 25:f827a8b7880e | 802 | |
kyleliangus | 9:1d8e4da058cd | 803 | //left_motor.forward(0.1); |
kyleliangus | 9:1d8e4da058cd | 804 | //right_motor.forward(0.1); |
christine222 | 25:f827a8b7880e | 805 | |
kyleliangus | 8:a0760acdc59e | 806 | // PA_1 is A of right |
kyleliangus | 8:a0760acdc59e | 807 | // PA_0 is B of right |
kyleliangus | 8:a0760acdc59e | 808 | // PA_5 is A of left |
kyleliangus | 8:a0760acdc59e | 809 | // PB_3 is B of left |
vanshg | 11:8fc2b703086b | 810 | //QEI encoder0( PA_5, PB_3, NC, PULSES, QEI::X4_ENCODING ); |
vanshg | 11:8fc2b703086b | 811 | // QEI encoder1( PA_1, PA_0, NC, PULSES, QEI::X4_ENCODING ); |
christine222 | 25:f827a8b7880e | 812 | |
vanshg | 10:810d1849da9d | 813 | // TODO: Setting all the registers and what not for Quadrature Encoders |
sahilmgandhi | 14:9e7bb03ddccb | 814 | /* RCC->APB1ENR |= 0x1001; // Enable clock for Tim2 (Bit 0) and Tim5 (Bit 3) |
sahilmgandhi | 14:9e7bb03ddccb | 815 | RCC->AHB1ENR |= 0x11; // Enable GPIO port clock enables for Tim2(A) and Tim5(B) |
sahilmgandhi | 14:9e7bb03ddccb | 816 | GPIOA->AFR[0] |= 0x10; // Set GPIO alternate function modes for Tim2 |
sahilmgandhi | 14:9e7bb03ddccb | 817 | GPIOB->AFR[0] |= 0x100; // Set GPIO alternate function modes for Tim5 |
sahilmgandhi | 14:9e7bb03ddccb | 818 | */ |
christine222 | 25:f827a8b7880e | 819 | |
kyleliangus | 12:5790e56a056f | 820 | // set GPIO pins to alternate for the pins corresponding to A/B for eacah encoder, and 2 alternate function registers need to be selected for each type |
kyleliangus | 12:5790e56a056f | 821 | // of alternate function specified |
kyleliangus | 12:5790e56a056f | 822 | // 4 modes sets AHB1ENR |
kyleliangus | 12:5790e56a056f | 823 | // Now TMR: enable clock with timer, APB1ENR |
kyleliangus | 12:5790e56a056f | 824 | // set period, autoreload value, ARR value 2^32-1, CR1 - TMR resets itself, ARPE and EN |
kyleliangus | 12:5790e56a056f | 825 | // |
kyleliangus | 12:5790e56a056f | 826 | // Encoder mode: disable timer before changing timer to encoder |
kyleliangus | 12:5790e56a056f | 827 | // CCMR1/2 1/2 depends on channel 1/2 or 3/4, depends on upper bits, depending which channels you use |
kyleliangus | 12:5790e56a056f | 828 | // CCMR sets sample rate and set the channel to input |
kyleliangus | 12:5790e56a056f | 829 | // CCER, which edge to trigger on, cannot be 11(not allowed for encoder mode), CCER for both channels |
kyleliangus | 12:5790e56a056f | 830 | // SMCR - encoder mode |
kyleliangus | 12:5790e56a056f | 831 | // CR1 reenabales |
kyleliangus | 12:5790e56a056f | 832 | // then read CNT reg of timer |
christine222 | 25:f827a8b7880e | 833 | |
christine222 | 25:f827a8b7880e | 834 | |
vanshg | 10:810d1849da9d | 835 | dipButton1.rise(&enableButton1); |
vanshg | 10:810d1849da9d | 836 | dipButton2.rise(&enableButton2); |
vanshg | 10:810d1849da9d | 837 | dipButton3.rise(&enableButton3); |
vanshg | 10:810d1849da9d | 838 | dipButton4.rise(&enableButton4); |
christine222 | 25:f827a8b7880e | 839 | |
vanshg | 10:810d1849da9d | 840 | dipButton1.fall(&disableButton1); |
vanshg | 10:810d1849da9d | 841 | dipButton2.fall(&disableButton2); |
vanshg | 10:810d1849da9d | 842 | dipButton3.fall(&disableButton3); |
vanshg | 10:810d1849da9d | 843 | dipButton4.fall(&disableButton4); |
sahilmgandhi | 7:6f5cb6377bd4 | 844 | |
christine222 | 25:f827a8b7880e | 845 | //right_motor.forward( 0.2 ); |
christine222 | 25:f827a8b7880e | 846 | //left_motor.forward( 0.2 ); |
christine222 | 25:f827a8b7880e | 847 | turnRight180(); |
christine222 | 25:f827a8b7880e | 848 | wait_ms(1500); |
christine222 | 24:e7063765d6f0 | 849 | |
christine222 | 24:e7063765d6f0 | 850 | |
christine222 | 3:880f15be8c72 | 851 | while (1) { |
christine222 | 23:690b0ca34ee9 | 852 | //wait_ms(1500); |
christine222 | 23:690b0ca34ee9 | 853 | //turnRight(); |
christine222 | 23:690b0ca34ee9 | 854 | //wait_ms(1500); |
christine222 | 23:690b0ca34ee9 | 855 | //turnLeft(); |
christine222 | 25:f827a8b7880e | 856 | |
christine222 | 25:f827a8b7880e | 857 | |
christine222 | 25:f827a8b7880e | 858 | // float ir2 = IRP_2.getSamples(100); |
christine222 | 25:f827a8b7880e | 859 | // float ir3 = IRP_3.getSamples(100); |
christine222 | 25:f827a8b7880e | 860 | // float ir1 = IRP_1.getSamples(100); |
christine222 | 25:f827a8b7880e | 861 | // float ir4 = IRP_4.getSamples(100); |
christine222 | 25:f827a8b7880e | 862 | |
christine222 | 25:f827a8b7880e | 863 | |
christine222 | 25:f827a8b7880e | 864 | // if( ir1 > IRP_1.sensorAvg*0.3 || ir4 > IRP_4.sensorAvg*0.3 ){ |
christine222 | 25:f827a8b7880e | 865 | // // almost to the end |
christine222 | 25:f827a8b7880e | 866 | // redLed.write(1); |
christine222 | 25:f827a8b7880e | 867 | // greenLed.write(1); |
christine222 | 25:f827a8b7880e | 868 | // blueLed.write(1); |
christine222 | 25:f827a8b7880e | 869 | // wait_ms(200); |
christine222 | 25:f827a8b7880e | 870 | // redLed.write(1); |
christine222 | 25:f827a8b7880e | 871 | // greenLed.write(1); |
christine222 | 25:f827a8b7880e | 872 | // blueLed.write(0); |
christine222 | 25:f827a8b7880e | 873 | // wait_ms(200); |
christine222 | 25:f827a8b7880e | 874 | // redLed.write(1); |
christine222 | 25:f827a8b7880e | 875 | // greenLed.write(0); |
christine222 | 25:f827a8b7880e | 876 | // blueLed.write(1); |
christine222 | 25:f827a8b7880e | 877 | // wait_ms(200); |
christine222 | 25:f827a8b7880e | 878 | // redLed.write(0); |
christine222 | 25:f827a8b7880e | 879 | // greenLed.write(1); |
christine222 | 25:f827a8b7880e | 880 | // blueLed.write(1); |
christine222 | 25:f827a8b7880e | 881 | |
christine222 | 25:f827a8b7880e | 882 | // }else if((ir3 < IRP_3.sensorAvg/(averageDivR)) && (ir2 < IRP_2.sensorAvg/(averageDivL)) ){ |
christine222 | 25:f827a8b7880e | 883 | // // both sides gone |
christine222 | 25:f827a8b7880e | 884 | // redLed.write(1); |
christine222 | 25:f827a8b7880e | 885 | // greenLed.write(1); |
christine222 | 25:f827a8b7880e | 886 | // blueLed.write(1); |
christine222 | 25:f827a8b7880e | 887 | // wait_ms(100); |
christine222 | 25:f827a8b7880e | 888 | // redLed.write(1); |
christine222 | 25:f827a8b7880e | 889 | // greenLed.write(1); |
christine222 | 25:f827a8b7880e | 890 | // blueLed.write(0); |
christine222 | 25:f827a8b7880e | 891 | // wait_ms(200); |
christine222 | 25:f827a8b7880e | 892 | // redLed.write(1); |
christine222 | 25:f827a8b7880e | 893 | // greenLed.write(1); |
christine222 | 25:f827a8b7880e | 894 | // blueLed.write(0); |
christine222 | 25:f827a8b7880e | 895 | // wait_ms(200); |
christine222 | 25:f827a8b7880e | 896 | // redLed.write(1); |
christine222 | 25:f827a8b7880e | 897 | // greenLed.write(1); |
christine222 | 25:f827a8b7880e | 898 | // blueLed.write(0); |
christine222 | 25:f827a8b7880e | 899 | // wait_ms(200); |
christine222 | 25:f827a8b7880e | 900 | // redLed.write(1); |
christine222 | 25:f827a8b7880e | 901 | // greenLed.write(1); |
christine222 | 25:f827a8b7880e | 902 | // blueLed.write(0); |
christine222 | 25:f827a8b7880e | 903 | // }else if (ir3 < IRP_3.sensorAvg/averageDivR){// right wall gone |
christine222 | 25:f827a8b7880e | 904 | // // RED RED RED RED RED |
christine222 | 25:f827a8b7880e | 905 | // redLed.write(0); |
christine222 | 25:f827a8b7880e | 906 | // greenLed.write(1); |
christine222 | 25:f827a8b7880e | 907 | // blueLed.write(1); |
christine222 | 25:f827a8b7880e | 908 | // }else if (ir2 < IRP_2.sensorAvg/averageDivL){// left wall gone |
christine222 | 25:f827a8b7880e | 909 | // // BLUE BLUE BLUE BLUE |
christine222 | 25:f827a8b7880e | 910 | // redLed.write(1); |
christine222 | 25:f827a8b7880e | 911 | // greenLed.write(1); |
christine222 | 25:f827a8b7880e | 912 | // blueLed.write(0); |
christine222 | 25:f827a8b7880e | 913 | // }else if ((ir2 > ((IRP_2.sensorAvg/initAverageL)*averageDivUpper)) && (ir3 > ((IRP_3.sensorAvg/initAverageR)*averageDivUpper))){ |
christine222 | 25:f827a8b7880e | 914 | // // both walls there |
christine222 | 25:f827a8b7880e | 915 | // redLed.write(1); |
christine222 | 25:f827a8b7880e | 916 | // greenLed.write(0); |
christine222 | 25:f827a8b7880e | 917 | // blueLed.write(1); |
christine222 | 25:f827a8b7880e | 918 | // } |
christine222 | 25:f827a8b7880e | 919 | |
christine222 | 25:f827a8b7880e | 920 | |
christine222 | 25:f827a8b7880e | 921 | |
christine222 | 25:f827a8b7880e | 922 | |
christine222 | 24:e7063765d6f0 | 923 | nCellEncoderAndIR(1); |
christine222 | 25:f827a8b7880e | 924 | wait_ms(1000); |
christine222 | 25:f827a8b7880e | 925 | |
christine222 | 25:f827a8b7880e | 926 | |
christine222 | 23:690b0ca34ee9 | 927 | // turnRight(); |
christine222 | 23:690b0ca34ee9 | 928 | // wait_ms(500); |
christine222 | 23:690b0ca34ee9 | 929 | // nCellEncoderAndIR(1); |
christine222 | 23:690b0ca34ee9 | 930 | // wait_ms(500); |
christine222 | 23:690b0ca34ee9 | 931 | // turnRight(); |
christine222 | 23:690b0ca34ee9 | 932 | // wait_ms(500); |
christine222 | 23:690b0ca34ee9 | 933 | // nCellEncoderAndIR(1); |
christine222 | 23:690b0ca34ee9 | 934 | // wait_ms(500); |
christine222 | 23:690b0ca34ee9 | 935 | // turnLeft(); |
christine222 | 23:690b0ca34ee9 | 936 | // wait_ms(500); |
christine222 | 23:690b0ca34ee9 | 937 | // nCellEncoderAndIR(2); |
christine222 | 23:690b0ca34ee9 | 938 | // wait_ms(500); |
christine222 | 23:690b0ca34ee9 | 939 | // turnRight(); |
christine222 | 23:690b0ca34ee9 | 940 | // wait_ms(500); |
christine222 | 23:690b0ca34ee9 | 941 | // nCellEncoderAndIR(1); |
christine222 | 23:690b0ca34ee9 | 942 | // wait_ms(500); |
christine222 | 23:690b0ca34ee9 | 943 | // turnRight(); |
christine222 | 23:690b0ca34ee9 | 944 | // wait_ms(500); |
christine222 | 23:690b0ca34ee9 | 945 | // nCellEncoderAndIR(5); |
christine222 | 23:690b0ca34ee9 | 946 | // break; |
christine222 | 23:690b0ca34ee9 | 947 | // turnRight180(); |
christine222 | 25:f827a8b7880e | 948 | |
christine222 | 25:f827a8b7880e | 949 | |
christine222 | 25:f827a8b7880e | 950 | |
christine222 | 23:690b0ca34ee9 | 951 | // int number = rand() % 4 + 1; |
christine222 | 23:690b0ca34ee9 | 952 | // switch(number){ |
christine222 | 23:690b0ca34ee9 | 953 | // case(1):{//turn right |
christine222 | 23:690b0ca34ee9 | 954 | // turnRight(); |
christine222 | 23:690b0ca34ee9 | 955 | // break; |
christine222 | 23:690b0ca34ee9 | 956 | // } |
christine222 | 23:690b0ca34ee9 | 957 | // case(2):{ // turn left |
christine222 | 23:690b0ca34ee9 | 958 | // turnLeft(); |
christine222 | 23:690b0ca34ee9 | 959 | // break; |
christine222 | 23:690b0ca34ee9 | 960 | // } |
christine222 | 23:690b0ca34ee9 | 961 | // case(3):{// keep going |
christine222 | 25:f827a8b7880e | 962 | |
christine222 | 23:690b0ca34ee9 | 963 | // break; |
christine222 | 23:690b0ca34ee9 | 964 | // } |
christine222 | 23:690b0ca34ee9 | 965 | // case(4):{// turnaround |
christine222 | 23:690b0ca34ee9 | 966 | // turnRight180(); |
christine222 | 23:690b0ca34ee9 | 967 | // break; |
christine222 | 23:690b0ca34ee9 | 968 | // } |
christine222 | 23:690b0ca34ee9 | 969 | // default:{// keep going |
christine222 | 23:690b0ca34ee9 | 970 | // break; |
christine222 | 23:690b0ca34ee9 | 971 | // } |
christine222 | 23:690b0ca34ee9 | 972 | // } |
christine222 | 25:f827a8b7880e | 973 | |
christine222 | 23:690b0ca34ee9 | 974 | // float irbase2 = IRP_2.sensorAvg/initAverageL/averageDivL; |
christine222 | 23:690b0ca34ee9 | 975 | // float irbase3 = IRP_3.sensorAvg/initAverageR/averageDivR; |
christine222 | 25:f827a8b7880e | 976 | |
christine222 | 23:690b0ca34ee9 | 977 | // float ir3 = IRP_2.getSamples(100)/initAverageL; |
christine222 | 23:690b0ca34ee9 | 978 | // float ir2 = IRP_3.getSamples(100)/initAverageR; |
christine222 | 22:681190ff98f0 | 979 | |
christine222 | 25:f827a8b7880e | 980 | //serial.printf("%f, %f \n", IRP_1.sensorAvg, IRP_4.sensorAvg); |
christine222 | 25:f827a8b7880e | 981 | //serial.printf("%f, %f \n", IRP_2.sensorAvg, IRP_3.sensorAvg); |
christine222 | 25:f827a8b7880e | 982 | //break; |
christine222 | 25:f827a8b7880e | 983 | |
christine222 | 25:f827a8b7880e | 984 | //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples(100), IRP_3.getSamples(100)); |
christine222 | 25:f827a8b7880e | 985 | //serial.printf("IRS= >: %f, %f \r\n", IRP_1.getSamples(100), IRP_4.getSamples(100)); |
christine222 | 25:f827a8b7880e | 986 | |
christine222 | 25:f827a8b7880e | 987 | |
christine222 | 25:f827a8b7880e | 988 | |
christine222 | 25:f827a8b7880e | 989 | |
christine222 | 23:690b0ca34ee9 | 990 | /* |
christine222 | 23:690b0ca34ee9 | 991 | counter2++; |
christine222 | 23:690b0ca34ee9 | 992 | counter3++; |
christine222 | 23:690b0ca34ee9 | 993 | ir2tot += IRP_2.getSamples(100); |
christine222 | 23:690b0ca34ee9 | 994 | ir3tot += IRP_3.getSamples(100); |
christine222 | 25:f827a8b7880e | 995 | |
christine222 | 25:f827a8b7880e | 996 | |
christine222 | 23:690b0ca34ee9 | 997 | ir2 = ir2tot/counter2; |
christine222 | 23:690b0ca34ee9 | 998 | ir3 = ir3tot/counter3; |
christine222 | 23:690b0ca34ee9 | 999 | |
christine222 | 25:f827a8b7880e | 1000 | |
christine222 | 23:690b0ca34ee9 | 1001 | serial.printf("IRS= >: %f, %f \r\n", ir2, ir3); |
christine222 | 23:690b0ca34ee9 | 1002 | */ |
christine222 | 23:690b0ca34ee9 | 1003 | //serial.printf("%f, %f \n", IRP_2.sensorAvg/initAverageL/averageDivL, IRP_3.sensorAvg/initAverageR/averageDivR); |
christine222 | 23:690b0ca34ee9 | 1004 | //serial.printf("IRBASEnowall= >: %f, %f \r\n", irbase2, irbase3); |
christine222 | 23:690b0ca34ee9 | 1005 | //break; |
christine222 | 23:690b0ca34ee9 | 1006 | //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples(100), IRP_3.getSamples(100)); |
christine222 | 23:690b0ca34ee9 | 1007 | //serial.printf("IRSAvg= >: %f, %f \r\n", ir2, ir3); |
christine222 | 23:690b0ca34ee9 | 1008 | //serial.printf("IRSAvg= >: %f, %f \r\n", IRP_2.sensorAvg, IRP_3.sensorAvg); |
christine222 | 25:f827a8b7880e | 1009 | |
christine222 | 25:f827a8b7880e | 1010 | |
christine222 | 23:690b0ca34ee9 | 1011 | //////////////////////////////////////////////////////////////// |
christine222 | 25:f827a8b7880e | 1012 | |
christine222 | 22:681190ff98f0 | 1013 | //nCellEncoderAndIR(3); |
christine222 | 22:681190ff98f0 | 1014 | //break; |
christine222 | 25:f827a8b7880e | 1015 | |
christine222 | 23:690b0ca34ee9 | 1016 | //serial.printf("IRS= >: %f, %f, %f, %f \r\n", IRP_1.getSamples( 100 ), IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ), IRP_4.getSamples(100)); |
christine222 | 23:690b0ca34ee9 | 1017 | |
christine222 | 25:f827a8b7880e | 1018 | |
christine222 | 25:f827a8b7880e | 1019 | |
christine222 | 21:9a6cb07bdcb6 | 1020 | //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 )); |
christine222 | 25:f827a8b7880e | 1021 | |
christine222 | 25:f827a8b7880e | 1022 | |
christine222 | 21:9a6cb07bdcb6 | 1023 | //break; |
christine222 | 21:9a6cb07bdcb6 | 1024 | // moveForwardCellEncoder(1); |
christine222 | 21:9a6cb07bdcb6 | 1025 | // wait(0.5); |
christine222 | 21:9a6cb07bdcb6 | 1026 | // handleTurns(); |
christine222 | 21:9a6cb07bdcb6 | 1027 | // wait(0.5); |
christine222 | 21:9a6cb07bdcb6 | 1028 | // moveForwardCellEncoder(1); |
christine222 | 21:9a6cb07bdcb6 | 1029 | // wait(0.5); |
christine222 | 21:9a6cb07bdcb6 | 1030 | // handleTurns(); |
christine222 | 21:9a6cb07bdcb6 | 1031 | //break; |
sahilmgandhi | 16:d9252437bd92 | 1032 | //pidOnEncoders(); |
sahilmgandhi | 20:82836745332e | 1033 | // moveForwardUntilWallIr(); |
kyleliangus | 8:a0760acdc59e | 1034 | //serial.printf("%i, %i, %i\n", gyro.getGyroX(), gyro.getGyroY(), gyro.getGyroZ()); |
kyleliangus | 15:b80555a4a8b9 | 1035 | //serial.printf("Pulse Count=> e0:%d, e1:%d \r\n", encoder0.getPulses(),encoder1.getPulses()); |
sahilmgandhi | 17:f713758f6238 | 1036 | // double currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ; |
christine222 | 21:9a6cb07bdcb6 | 1037 | //serial.printf("IRS= >: %f, %f, %f, %f, %f \r\n", IRP_1.getSamples( 100 ), IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ), IRP_4.getSamples(100), currentError ); |
christine222 | 25:f827a8b7880e | 1038 | |
christine222 | 3:880f15be8c72 | 1039 | //reading = Rec_4.read(); |
christine222 | 3:880f15be8c72 | 1040 | // serial.printf("reading: %f\n", reading); |
christine222 | 3:880f15be8c72 | 1041 | } |
christine222 | 25:f827a8b7880e | 1042 | } |