MacroRat / MouseCode

Mouse code for the MacroRat

Dependencies:   ITG3200 QEI

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?

UserRevisionLine numberNew 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 }