MacroRat / MouseCode

Mouse code for the MacroRat

Dependencies:   ITG3200 QEI

main.cpp@23:690b0ca34ee9, 2017-05-20 (annotated)

Committer:
christine222
Date:
Sat May 20 09:11:08 2017 +0000
Revision:
23:690b0ca34ee9
Parent:
22:681190ff98f0
Child:
24:e7063765d6f0
ncellencoderirwall function working for all walls (both, 1 side, no sides) need to tune detecting the sides though, there's a very precise threshold that we need to find

Who changed what in which revision?

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