MacroRat / MouseCode

Mouse code for the MacroRat

Dependencies:   ITG3200 QEI

main.cpp@24:e7063765d6f0, 2017-05-21 (annotated)

Committer:
christine222
Date:
Sun May 21 01:04:26 2017 +0000
Revision:
24:e7063765d6f0
Parent:
23:690b0ca34ee9
Child:
25:f827a8b7880e
front face start

Who changed what in which revision?

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