MacroRat / MouseCode

Mouse code for the MacroRat

Dependencies:   ITG3200 QEI

main.cpp@21:9a6cb07bdcb6, 2017-05-18 (annotated)

Committer:
christine222
Date:
Thu May 18 02:52:22 2017 +0000
Revision:
21:9a6cb07bdcb6
Parent:
20:82836745332e
Child:
22:681190ff98f0
better PID constants for forwardWallIR and added no wall encoder pid moveForwardEncoder() function that is used in forwardWallIR()

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sahilmgandhi 0:a03c771ab78e 1 #include "mbed.h"
kyleliangus 4:b5b7836ca2b0 2
kyleliangus 6:3d68fedd6fd9 3 #include "irpair.h"
kyleliangus 4:b5b7836ca2b0 4 #include "main.h"
kyleliangus 4:b5b7836ca2b0 5 #include "motor.h"
kyleliangus 4:b5b7836ca2b0 6
sahilmgandhi 0:a03c771ab78e 7 #include <stdlib.h>
sahilmgandhi 1:8a4b2f573923 8 #include "ITG3200.h"
sahilmgandhi 14:9e7bb03ddccb 9 #include "stm32f4xx.h"
sahilmgandhi 7:6f5cb6377bd4 10 #include "QEI.h"
sahilmgandhi 0:a03c771ab78e 11
vanshg 11:8fc2b703086b 12 /* Constants for when HIGH_PWM_VOLTAGE = 0.2
kyleliangus 9:1d8e4da058cd 13 #define IP_CONSTANT 6
kyleliangus 9:1d8e4da058cd 14 #define II_CONSTANT 0
kyleliangus 9:1d8e4da058cd 15 #define ID_CONSTANT 1
vanshg 11:8fc2b703086b 16 */
sahilmgandhi 0:a03c771ab78e 17
vanshg 11:8fc2b703086b 18 // Constants for when HIGH_PWM_VOLTAGE = 0.1
christine222 21:9a6cb07bdcb6 19 // #define IP_CONSTANT 8.85
christine222 21:9a6cb07bdcb6 20 // #define II_CONSTANT 0.005
christine222 21:9a6cb07bdcb6 21 // #define ID_CONSTANT 3.15
christine222 21:9a6cb07bdcb6 22 #define IP_CONSTANT 7.85
christine222 21:9a6cb07bdcb6 23 #define II_CONSTANT 0.095
christine222 21:9a6cb07bdcb6 24 #define ID_CONSTANT 20.85
sahilmgandhi 14:9e7bb03ddccb 25
sahilmgandhi 14:9e7bb03ddccb 26 const int desiredCountR = 1400;
sahilmgandhi 14:9e7bb03ddccb 27 const int desiredCountL = 1475;
christine222 13:2032db00f168 28
sahilmgandhi 16:d9252437bd92 29 const int oneCellCount = 5400;
sahilmgandhi 20:82836745332e 30 const int oneCellCountMomentum = 4700; // one cell count is actually approximately 5400, but this value is considering momentum!
sahilmgandhi 16:d9252437bd92 31
sahilmgandhi 17:f713758f6238 32 float receiverOneReading = 0.0;
sahilmgandhi 17:f713758f6238 33 float receiverTwoReading = 0.0;
sahilmgandhi 17:f713758f6238 34 float receiverThreeReading = 0.0;
sahilmgandhi 17:f713758f6238 35 float receiverFourReading = 0.0;
sahilmgandhi 17:f713758f6238 36
sahilmgandhi 16:d9252437bd92 37 void pidOnEncoders();
christine222 13:2032db00f168 38
christine222 21:9a6cb07bdcb6 39
sahilmgandhi 14:9e7bb03ddccb 40 void turnLeft()
sahilmgandhi 14:9e7bb03ddccb 41 {
christine222 13:2032db00f168 42 double speed0 = 0.15;
sahilmgandhi 14:9e7bb03ddccb 43 double speed1 = -0.15;
christine222 13:2032db00f168 44
christine222 13:2032db00f168 45 int counter = 0;
sahilmgandhi 14:9e7bb03ddccb 46 int initial0 = encoder0.getPulses();
sahilmgandhi 14:9e7bb03ddccb 47 int initial1 = encoder1.getPulses();
christine222 13:2032db00f168 48
sahilmgandhi 14:9e7bb03ddccb 49 int desiredCount0 = initial0 - desiredCountL;
sahilmgandhi 14:9e7bb03ddccb 50 int desiredCount1 = initial1 + desiredCountL;
christine222 13:2032db00f168 51
sahilmgandhi 14:9e7bb03ddccb 52 int count0 = initial0;
sahilmgandhi 14:9e7bb03ddccb 53 int count1 = initial1;
christine222 13:2032db00f168 54
christine222 13:2032db00f168 55 double error0 = count0 - desiredCount0;
christine222 13:2032db00f168 56 double error1 = count1 - desiredCount1;
christine222 13:2032db00f168 57
christine222 13:2032db00f168 58
sahilmgandhi 14:9e7bb03ddccb 59 while(1) {
sahilmgandhi 14:9e7bb03ddccb 60
sahilmgandhi 14:9e7bb03ddccb 61 if(!(abs(error0) < 1) && !(abs(error1) < 1)) {
christine222 13:2032db00f168 62 count0 = encoder0.getPulses();
christine222 13:2032db00f168 63 count1 = encoder1.getPulses();
christine222 13:2032db00f168 64
sahilmgandhi 14:9e7bb03ddccb 65 error0 = count0 - desiredCount0;
sahilmgandhi 14:9e7bb03ddccb 66 error1 = count1 - desiredCount1;
christine222 13:2032db00f168 67
christine222 13:2032db00f168 68 right_motor.move(speed0);
christine222 13:2032db00f168 69 left_motor.move(speed1);
christine222 13:2032db00f168 70 counter = 0;
sahilmgandhi 14:9e7bb03ddccb 71 } else {
christine222 13:2032db00f168 72 counter++;
christine222 13:2032db00f168 73 right_motor.brake();
christine222 13:2032db00f168 74 left_motor.brake();
christine222 13:2032db00f168 75 }
christine222 13:2032db00f168 76
sahilmgandhi 14:9e7bb03ddccb 77 if (counter > 60) {
christine222 13:2032db00f168 78 break;
christine222 13:2032db00f168 79 }
sahilmgandhi 14:9e7bb03ddccb 80 }
christine222 13:2032db00f168 81
christine222 13:2032db00f168 82 right_motor.brake();
christine222 13:2032db00f168 83 left_motor.brake();
sahilmgandhi 17:f713758f6238 84 turnFlag = 0; // zeroing out the flags!
sahilmgandhi 20:82836745332e 85 currDir -= 1;
christine222 13:2032db00f168 86 }
kyleliangus 9:1d8e4da058cd 87
sahilmgandhi 14:9e7bb03ddccb 88 void turnRight()
sahilmgandhi 14:9e7bb03ddccb 89 {
sahilmgandhi 14:9e7bb03ddccb 90 double speed0 = -0.15;
kyleliangus 12:5790e56a056f 91 double speed1 = 0.15;
christine222 13:2032db00f168 92
christine222 13:2032db00f168 93 int counter = 0;
sahilmgandhi 14:9e7bb03ddccb 94 int initial0 = encoder0.getPulses();
sahilmgandhi 14:9e7bb03ddccb 95 int initial1 = encoder1.getPulses();
christine222 13:2032db00f168 96
sahilmgandhi 14:9e7bb03ddccb 97 int desiredCount0 = initial0 + desiredCountR;
sahilmgandhi 14:9e7bb03ddccb 98 int desiredCount1 = initial1 - desiredCountR;
christine222 13:2032db00f168 99
sahilmgandhi 14:9e7bb03ddccb 100 int count0 = initial0;
sahilmgandhi 14:9e7bb03ddccb 101 int count1 = initial1;
kyleliangus 12:5790e56a056f 102
christine222 13:2032db00f168 103 double error0 = count0 - desiredCount0;
christine222 13:2032db00f168 104 double error1 = count1 - desiredCount1;
christine222 13:2032db00f168 105
sahilmgandhi 14:9e7bb03ddccb 106 while(1) {
sahilmgandhi 14:9e7bb03ddccb 107
sahilmgandhi 14:9e7bb03ddccb 108 if(!(abs(error0) < 1) && !(abs(error1) < 1)) {
christine222 13:2032db00f168 109 count0 = encoder0.getPulses();
christine222 13:2032db00f168 110 count1 = encoder1.getPulses();
christine222 13:2032db00f168 111
sahilmgandhi 14:9e7bb03ddccb 112 error0 = count0 - desiredCount0;
sahilmgandhi 14:9e7bb03ddccb 113 error1 = count1 - desiredCount1;
kyleliangus 12:5790e56a056f 114
christine222 13:2032db00f168 115 right_motor.move(speed0);
christine222 13:2032db00f168 116 left_motor.move(speed1);
christine222 13:2032db00f168 117 counter = 0;
sahilmgandhi 14:9e7bb03ddccb 118 } else {
christine222 13:2032db00f168 119 counter++;
christine222 13:2032db00f168 120 right_motor.brake();
christine222 13:2032db00f168 121 left_motor.brake();
christine222 13:2032db00f168 122 }
kyleliangus 12:5790e56a056f 123
sahilmgandhi 14:9e7bb03ddccb 124 if (counter > 60) {
christine222 13:2032db00f168 125 break;
christine222 13:2032db00f168 126 }
christine222 13:2032db00f168 127 }
christine222 13:2032db00f168 128
christine222 13:2032db00f168 129 right_motor.brake();
christine222 13:2032db00f168 130 left_motor.brake();
sahilmgandhi 17:f713758f6238 131 turnFlag = 0;
sahilmgandhi 20:82836745332e 132 currDir += 1;
christine222 13:2032db00f168 133 }
christine222 13:2032db00f168 134
sahilmgandhi 14:9e7bb03ddccb 135 void turnLeft180()
sahilmgandhi 14:9e7bb03ddccb 136 {
christine222 13:2032db00f168 137 double speed0 = 0.15;
sahilmgandhi 14:9e7bb03ddccb 138 double speed1 = -0.15;
christine222 13:2032db00f168 139
christine222 13:2032db00f168 140 int counter = 0;
sahilmgandhi 14:9e7bb03ddccb 141 int initial0 = encoder0.getPulses();
sahilmgandhi 14:9e7bb03ddccb 142 int initial1 = encoder1.getPulses();
kyleliangus 12:5790e56a056f 143
sahilmgandhi 14:9e7bb03ddccb 144 int desiredCount0 = initial0 - desiredCountL*2;
sahilmgandhi 14:9e7bb03ddccb 145 int desiredCount1 = initial1 + desiredCountL*2;
christine222 13:2032db00f168 146
sahilmgandhi 14:9e7bb03ddccb 147 int count0 = initial0;
sahilmgandhi 14:9e7bb03ddccb 148 int count1 = initial1;
christine222 13:2032db00f168 149
christine222 13:2032db00f168 150 double error0 = count0 - desiredCount0;
christine222 13:2032db00f168 151 double error1 = count1 - desiredCount1;
christine222 13:2032db00f168 152
christine222 13:2032db00f168 153
sahilmgandhi 14:9e7bb03ddccb 154 while(1) {
kyleliangus 15:b80555a4a8b9 155
sahilmgandhi 14:9e7bb03ddccb 156 if(!(abs(error0) < 1) && !(abs(error1) < 1)) {
christine222 13:2032db00f168 157 count0 = encoder0.getPulses();
christine222 13:2032db00f168 158 count1 = encoder1.getPulses();
christine222 13:2032db00f168 159
sahilmgandhi 14:9e7bb03ddccb 160 error0 = count0 - desiredCount0;
sahilmgandhi 14:9e7bb03ddccb 161 error1 = count1 - desiredCount1;
kyleliangus 12:5790e56a056f 162
christine222 13:2032db00f168 163 right_motor.move(speed0);
christine222 13:2032db00f168 164 left_motor.move(speed1);
christine222 13:2032db00f168 165 counter = 0;
sahilmgandhi 14:9e7bb03ddccb 166 } else {
christine222 13:2032db00f168 167 counter++;
christine222 13:2032db00f168 168 right_motor.brake();
christine222 13:2032db00f168 169 left_motor.brake();
christine222 13:2032db00f168 170 }
christine222 13:2032db00f168 171
sahilmgandhi 14:9e7bb03ddccb 172 if (counter > 60) {
christine222 13:2032db00f168 173 break;
christine222 13:2032db00f168 174 }
kyleliangus 12:5790e56a056f 175 }
kyleliangus 12:5790e56a056f 176
kyleliangus 12:5790e56a056f 177 right_motor.brake();
kyleliangus 12:5790e56a056f 178 left_motor.brake();
sahilmgandhi 20:82836745332e 179 currDir -= 2;
kyleliangus 12:5790e56a056f 180 }
kyleliangus 12:5790e56a056f 181
sahilmgandhi 14:9e7bb03ddccb 182 void turnRight180()
sahilmgandhi 14:9e7bb03ddccb 183 {
sahilmgandhi 14:9e7bb03ddccb 184 double speed0 = -0.15;
sahilmgandhi 14:9e7bb03ddccb 185 double speed1 = 0.15;
sahilmgandhi 14:9e7bb03ddccb 186
sahilmgandhi 14:9e7bb03ddccb 187 int counter = 0;
sahilmgandhi 14:9e7bb03ddccb 188 int initial0 = encoder0.getPulses();
sahilmgandhi 14:9e7bb03ddccb 189 int initial1 = encoder1.getPulses();
sahilmgandhi 14:9e7bb03ddccb 190
sahilmgandhi 14:9e7bb03ddccb 191 int desiredCount0 = initial0 + desiredCountR*2;
sahilmgandhi 14:9e7bb03ddccb 192 int desiredCount1 = initial1 - desiredCountR*2;
sahilmgandhi 14:9e7bb03ddccb 193
sahilmgandhi 14:9e7bb03ddccb 194 int count0 = initial0;
sahilmgandhi 14:9e7bb03ddccb 195 int count1 = initial1;
sahilmgandhi 14:9e7bb03ddccb 196
sahilmgandhi 14:9e7bb03ddccb 197 double error0 = count0 - desiredCount0;
sahilmgandhi 14:9e7bb03ddccb 198 double error1 = count1 - desiredCount1;
sahilmgandhi 14:9e7bb03ddccb 199
sahilmgandhi 14:9e7bb03ddccb 200
sahilmgandhi 14:9e7bb03ddccb 201 while(1) {
sahilmgandhi 14:9e7bb03ddccb 202
sahilmgandhi 14:9e7bb03ddccb 203 if(!(abs(error0) < 1) && !(abs(error1) < 1)) {
sahilmgandhi 14:9e7bb03ddccb 204 count0 = encoder0.getPulses();
sahilmgandhi 14:9e7bb03ddccb 205 count1 = encoder1.getPulses();
sahilmgandhi 14:9e7bb03ddccb 206
sahilmgandhi 14:9e7bb03ddccb 207 error0 = count0 - desiredCount0;
sahilmgandhi 14:9e7bb03ddccb 208 error1 = count1 - desiredCount1;
sahilmgandhi 14:9e7bb03ddccb 209
sahilmgandhi 14:9e7bb03ddccb 210 right_motor.move(speed0);
sahilmgandhi 14:9e7bb03ddccb 211 left_motor.move(speed1);
sahilmgandhi 14:9e7bb03ddccb 212 counter = 0;
sahilmgandhi 14:9e7bb03ddccb 213 } else {
sahilmgandhi 14:9e7bb03ddccb 214 counter++;
sahilmgandhi 14:9e7bb03ddccb 215 right_motor.brake();
sahilmgandhi 14:9e7bb03ddccb 216 left_motor.brake();
sahilmgandhi 14:9e7bb03ddccb 217 }
sahilmgandhi 14:9e7bb03ddccb 218
sahilmgandhi 14:9e7bb03ddccb 219 if (counter > 60) {
sahilmgandhi 14:9e7bb03ddccb 220 break;
sahilmgandhi 14:9e7bb03ddccb 221 }
sahilmgandhi 14:9e7bb03ddccb 222 }
sahilmgandhi 14:9e7bb03ddccb 223 right_motor.brake();
sahilmgandhi 14:9e7bb03ddccb 224 left_motor.brake();
sahilmgandhi 20:82836745332e 225 currDir += 2;
sahilmgandhi 14:9e7bb03ddccb 226 }
sahilmgandhi 14:9e7bb03ddccb 227
sahilmgandhi 17:f713758f6238 228 void moveForwardCellEncoder(double cellNum){
sahilmgandhi 17:f713758f6238 229 int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellNum;
sahilmgandhi 17:f713758f6238 230 int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellNum;
sahilmgandhi 14:9e7bb03ddccb 231
sahilmgandhi 20:82836745332e 232 left_motor.forward(0.125);
sahilmgandhi 20:82836745332e 233 right_motor.forward(0.095);
sahilmgandhi 19:7b66a518b6f8 234 wait_ms(1);
sahilmgandhi 16:d9252437bd92 235 while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1){
sahilmgandhi 20:82836745332e 236 receiverTwoReading = IRP_2.getSamples(100);
sahilmgandhi 20:82836745332e 237 receiverThreeReading = IRP_3.getSamples(100);
sahilmgandhi 20:82836745332e 238 // serial.printf("Average 2: %f Average 3: %f Sensor 2: %f Sensor 3: %f\n", IRP_2.sensorAvg, IRP_3.sensorAvg, receiverTwoReading, receiverThreeReading);
sahilmgandhi 20:82836745332e 239 if (receiverThreeReading < IRP_3.sensorAvg/4){
sahilmgandhi 20:82836745332e 240 // redLed.write(1);
sahilmgandhi 20:82836745332e 241 // blueLed.write(0);
sahilmgandhi 19:7b66a518b6f8 242 turnFlag |= RIGHT_FLAG;
sahilmgandhi 20:82836745332e 243 }
sahilmgandhi 20:82836745332e 244 else if (receiverTwoReading < IRP_2.sensorAvg/4){
sahilmgandhi 20:82836745332e 245 // redLed.write(0);
sahilmgandhi 20:82836745332e 246 // blueLed.write(1);
sahilmgandhi 17:f713758f6238 247 turnFlag |= LEFT_FLAG;
sahilmgandhi 20:82836745332e 248 }
sahilmgandhi 16:d9252437bd92 249 pidOnEncoders();
sahilmgandhi 16:d9252437bd92 250 }
sahilmgandhi 14:9e7bb03ddccb 251
sahilmgandhi 16:d9252437bd92 252 left_motor.brake();
sahilmgandhi 14:9e7bb03ddccb 253 right_motor.brake();
sahilmgandhi 14:9e7bb03ddccb 254 }
sahilmgandhi 14:9e7bb03ddccb 255
sahilmgandhi 17:f713758f6238 256 void handleTurns(){
sahilmgandhi 17:f713758f6238 257 if (turnFlag == 0x1){
sahilmgandhi 19:7b66a518b6f8 258 // moveForwardCellEncoder(0.3);
sahilmgandhi 17:f713758f6238 259 turnLeft();
sahilmgandhi 17:f713758f6238 260 }
sahilmgandhi 17:f713758f6238 261 else if (turnFlag == 0x2){
sahilmgandhi 19:7b66a518b6f8 262 // moveForwardCellEncoder(0.3);
sahilmgandhi 17:f713758f6238 263 turnRight();
sahilmgandhi 17:f713758f6238 264 }
sahilmgandhi 17:f713758f6238 265 else if (turnFlag == 0x3){
sahilmgandhi 19:7b66a518b6f8 266 // moveForwardCellEncoder(0.3);
sahilmgandhi 17:f713758f6238 267 turnLeft();
sahilmgandhi 19:7b66a518b6f8 268 turnRight();
sahilmgandhi 17:f713758f6238 269 }
sahilmgandhi 17:f713758f6238 270 }
sahilmgandhi 17:f713758f6238 271
christine222 21:9a6cb07bdcb6 272 void pidBrake(){
christine222 21:9a6cb07bdcb6 273
christine222 21:9a6cb07bdcb6 274 int count0;
christine222 21:9a6cb07bdcb6 275 int count1;
christine222 21:9a6cb07bdcb6 276 count0 = encoder0.getPulses();
christine222 21:9a6cb07bdcb6 277 count1 = encoder1.getPulses();
christine222 21:9a6cb07bdcb6 278 int initial0 = count0;
christine222 21:9a6cb07bdcb6 279 int initial1 = count1;
christine222 21:9a6cb07bdcb6 280 double kp = 0.00011;
christine222 21:9a6cb07bdcb6 281
christine222 21:9a6cb07bdcb6 282
christine222 21:9a6cb07bdcb6 283
christine222 21:9a6cb07bdcb6 284 int error = count0 - count1;
christine222 21:9a6cb07bdcb6 285
christine222 21:9a6cb07bdcb6 286 int counter = 0;
christine222 21:9a6cb07bdcb6 287 right_motor.move(0.7);
christine222 21:9a6cb07bdcb6 288 left_motor.move(0.7);
christine222 21:9a6cb07bdcb6 289
christine222 21:9a6cb07bdcb6 290 double speed0 = 0.7;
christine222 21:9a6cb07bdcb6 291 double speed1 = 0.7;
christine222 21:9a6cb07bdcb6 292
christine222 21:9a6cb07bdcb6 293 while(1)
christine222 21:9a6cb07bdcb6 294 {
christine222 21:9a6cb07bdcb6 295 if(abs(error) < 3){
christine222 21:9a6cb07bdcb6 296 right_motor.brake();
christine222 21:9a6cb07bdcb6 297 left_motor.brake();
christine222 21:9a6cb07bdcb6 298 counter++;
christine222 21:9a6cb07bdcb6 299 }else{
christine222 21:9a6cb07bdcb6 300 count0 = encoder0.getPulses() - initial0;
christine222 21:9a6cb07bdcb6 301 count1 = encoder1.getPulses() - initial1;
christine222 21:9a6cb07bdcb6 302 error = count0 - count1;
christine222 21:9a6cb07bdcb6 303 speed0 = -error*kp + speed0;
christine222 21:9a6cb07bdcb6 304 speed1 = error*kp + speed1;
christine222 21:9a6cb07bdcb6 305
christine222 21:9a6cb07bdcb6 306 right_motor.move(speed0);
christine222 21:9a6cb07bdcb6 307 left_motor.move(speed1);
christine222 21:9a6cb07bdcb6 308
christine222 21:9a6cb07bdcb6 309 counter = 0;
christine222 21:9a6cb07bdcb6 310 }
christine222 21:9a6cb07bdcb6 311 if (counter > 10){
christine222 21:9a6cb07bdcb6 312 break;
christine222 21:9a6cb07bdcb6 313 }
christine222 21:9a6cb07bdcb6 314
christine222 21:9a6cb07bdcb6 315 }
christine222 21:9a6cb07bdcb6 316 return;
christine222 21:9a6cb07bdcb6 317 }
christine222 21:9a6cb07bdcb6 318
christine222 21:9a6cb07bdcb6 319 void moveForwardEncoder(){
christine222 21:9a6cb07bdcb6 320
christine222 21:9a6cb07bdcb6 321 int count0;
christine222 21:9a6cb07bdcb6 322 int count1;
christine222 21:9a6cb07bdcb6 323 count0 = encoder0.getPulses();
christine222 21:9a6cb07bdcb6 324 count1 = encoder1.getPulses();
christine222 21:9a6cb07bdcb6 325 int initial1 = count1;
christine222 21:9a6cb07bdcb6 326 int initial0 = count0;
christine222 21:9a6cb07bdcb6 327 int diff = count0 - count1;
christine222 21:9a6cb07bdcb6 328 double kp = 0.00015;
christine222 21:9a6cb07bdcb6 329 double kd = 0.00019;
christine222 21:9a6cb07bdcb6 330 int prev = 0;
christine222 21:9a6cb07bdcb6 331
christine222 21:9a6cb07bdcb6 332
christine222 21:9a6cb07bdcb6 333
christine222 21:9a6cb07bdcb6 334 double speed0 = 0.1;
christine222 21:9a6cb07bdcb6 335 double speed1 = 0.13;
christine222 21:9a6cb07bdcb6 336 right_motor.move(speed0);
christine222 21:9a6cb07bdcb6 337 left_motor.move(speed1);
christine222 21:9a6cb07bdcb6 338
christine222 21:9a6cb07bdcb6 339
christine222 21:9a6cb07bdcb6 340 while((IRP_2.getSamples(SAMPLE_NUM) < 0.005 || IRP_3.getSamples(SAMPLE_NUM) < 0.005) && ((IRP_1.getSamples( SAMPLE_NUM ) + IRP_4.getSamples( SAMPLE_NUM ) )/2 < 0.25) ) {
christine222 21:9a6cb07bdcb6 341
christine222 21:9a6cb07bdcb6 342 //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ));
christine222 21:9a6cb07bdcb6 343
christine222 21:9a6cb07bdcb6 344 count0 = encoder0.getPulses() - initial0;
christine222 21:9a6cb07bdcb6 345 count1 = encoder1.getPulses() - initial1;
christine222 21:9a6cb07bdcb6 346 int x = count0 - count1;
christine222 21:9a6cb07bdcb6 347 //double d = kp * x + kd * ( x - prev );
christine222 21:9a6cb07bdcb6 348 double kppart = kp * x;
christine222 21:9a6cb07bdcb6 349 double kdpart = kd * (x-prev);
christine222 21:9a6cb07bdcb6 350 double d = kppart + kdpart;
christine222 21:9a6cb07bdcb6 351
christine222 21:9a6cb07bdcb6 352 //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 353 if( x < diff - 40 ) // count1 is bigger, right wheel pushed forward
christine222 21:9a6cb07bdcb6 354 {
christine222 21:9a6cb07bdcb6 355 left_motor.move( speed1-0.8*d );
christine222 21:9a6cb07bdcb6 356 right_motor.move( speed0+d );
christine222 21:9a6cb07bdcb6 357 }
christine222 21:9a6cb07bdcb6 358 else if( x > diff + 40 )
christine222 21:9a6cb07bdcb6 359 {
christine222 21:9a6cb07bdcb6 360 left_motor.move( speed1-0.8*d );
christine222 21:9a6cb07bdcb6 361 right_motor.move( speed0+d );
christine222 21:9a6cb07bdcb6 362 }
christine222 21:9a6cb07bdcb6 363 // else
christine222 21:9a6cb07bdcb6 364 // {
christine222 21:9a6cb07bdcb6 365 // left_motor.brake();
christine222 21:9a6cb07bdcb6 366 // right_motor.brake();
christine222 21:9a6cb07bdcb6 367 // }
christine222 21:9a6cb07bdcb6 368 prev = x;
christine222 21:9a6cb07bdcb6 369 }
christine222 21:9a6cb07bdcb6 370
christine222 21:9a6cb07bdcb6 371 //pidOnEncoders();
christine222 21:9a6cb07bdcb6 372 pidBrake();
christine222 21:9a6cb07bdcb6 373 //right_motor.brake();
christine222 21:9a6cb07bdcb6 374 //left_motor.brake();
christine222 21:9a6cb07bdcb6 375 return;
christine222 21:9a6cb07bdcb6 376 }
christine222 21:9a6cb07bdcb6 377
sahilmgandhi 14:9e7bb03ddccb 378 void moveForwardUntilWallIr()
sahilmgandhi 14:9e7bb03ddccb 379 {
kyleliangus 9:1d8e4da058cd 380 double currentError = 0;
kyleliangus 9:1d8e4da058cd 381 double previousError = 0;
kyleliangus 9:1d8e4da058cd 382 double derivError = 0;
kyleliangus 9:1d8e4da058cd 383 double sumError = 0;
sahilmgandhi 14:9e7bb03ddccb 384
vanshg 11:8fc2b703086b 385 double HIGH_PWM_VOLTAGE = 0.1;
sahilmgandhi 14:9e7bb03ddccb 386
christine222 21:9a6cb07bdcb6 387 double rightSpeed = 0.14;
christine222 21:9a6cb07bdcb6 388 double leftSpeed = 0.17;
sahilmgandhi 14:9e7bb03ddccb 389
kyleliangus 9:1d8e4da058cd 390 float ir2 = IRP_2.getSamples( SAMPLE_NUM );
kyleliangus 9:1d8e4da058cd 391 float ir3 = IRP_3.getSamples( SAMPLE_NUM );
sahilmgandhi 14:9e7bb03ddccb 392
vanshg 11:8fc2b703086b 393 int count = encoder0.getPulses();
christine222 21:9a6cb07bdcb6 394 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 395
christine222 21:9a6cb07bdcb6 396 if((IRP_2.getSamples(SAMPLE_NUM) < 0.005 || IRP_3.getSamples(SAMPLE_NUM) < 0.005)) {
christine222 21:9a6cb07bdcb6 397 serial.printf("asdfasdf \n");
christine222 21:9a6cb07bdcb6 398 moveForwardEncoder();
christine222 21:9a6cb07bdcb6 399 }else{
christine222 21:9a6cb07bdcb6 400 // will move forward using encoders only
christine222 21:9a6cb07bdcb6 401 // if cell ahead doesn't have a wall on either one side or both sides
christine222 21:9a6cb07bdcb6 402
christine222 21:9a6cb07bdcb6 403 int pulseCount = (encoder0.getPulses()-count) % 5600;
christine222 21:9a6cb07bdcb6 404 if (pulseCount > 5400 && pulseCount < 5800) {
christine222 21:9a6cb07bdcb6 405 blueLed.write(0);
christine222 21:9a6cb07bdcb6 406 } else {
christine222 21:9a6cb07bdcb6 407 blueLed.write(1);
christine222 21:9a6cb07bdcb6 408 }
christine222 21:9a6cb07bdcb6 409 sumError += currentError;
christine222 21:9a6cb07bdcb6 410 currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ;
christine222 21:9a6cb07bdcb6 411 derivError = currentError - previousError;
christine222 21:9a6cb07bdcb6 412 double PIDSum = IP_CONSTANT*currentError + II_CONSTANT*sumError + ID_CONSTANT*derivError;
christine222 21:9a6cb07bdcb6 413 if (PIDSum > 0) { // this means the leftWheel is faster than the right. So right speeds up, left slows down
christine222 21:9a6cb07bdcb6 414 rightSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE);
christine222 21:9a6cb07bdcb6 415 leftSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE);
christine222 21:9a6cb07bdcb6 416 } else { // r is faster than L. speed up l, slow down r
christine222 21:9a6cb07bdcb6 417 rightSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE);
christine222 21:9a6cb07bdcb6 418 leftSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE);
christine222 21:9a6cb07bdcb6 419 }
christine222 21:9a6cb07bdcb6 420
christine222 21:9a6cb07bdcb6 421 if (leftSpeed > 0.30) leftSpeed = 0.30;
christine222 21:9a6cb07bdcb6 422 if (leftSpeed < 0) leftSpeed = 0;
christine222 21:9a6cb07bdcb6 423 if (rightSpeed > 0.30) rightSpeed = 0.30;
christine222 21:9a6cb07bdcb6 424 if (rightSpeed < 0) rightSpeed = 0;
christine222 21:9a6cb07bdcb6 425
christine222 21:9a6cb07bdcb6 426 right_motor.forward(rightSpeed);
christine222 21:9a6cb07bdcb6 427 left_motor.forward(leftSpeed);
christine222 21:9a6cb07bdcb6 428
christine222 21:9a6cb07bdcb6 429 previousError = currentError;
christine222 21:9a6cb07bdcb6 430
christine222 21:9a6cb07bdcb6 431 ir2 = IRP_2.getSamples( SAMPLE_NUM );
christine222 21:9a6cb07bdcb6 432 ir3 = IRP_3.getSamples( SAMPLE_NUM );
christine222 21:9a6cb07bdcb6 433
kyleliangus 9:1d8e4da058cd 434 }
sahilmgandhi 14:9e7bb03ddccb 435
christine222 21:9a6cb07bdcb6 436 //backward();
christine222 21:9a6cb07bdcb6 437 wait_ms(40);
christine222 21:9a6cb07bdcb6 438 //brake();
sahilmgandhi 14:9e7bb03ddccb 439
christine222 21:9a6cb07bdcb6 440 left_motor.brake();
christine222 21:9a6cb07bdcb6 441 right_motor.brake();
kyleliangus 9:1d8e4da058cd 442 }
vanshg 10:810d1849da9d 443 }
vanshg 10:810d1849da9d 444
sahilmgandhi 14:9e7bb03ddccb 445 void printDipFlag()
sahilmgandhi 14:9e7bb03ddccb 446 {
vanshg 11:8fc2b703086b 447 if (DEBUGGING) serial.printf("Flag value is %d", dipFlags);
vanshg 11:8fc2b703086b 448 }
vanshg 11:8fc2b703086b 449
sahilmgandhi 14:9e7bb03ddccb 450 void enableButton1()
sahilmgandhi 14:9e7bb03ddccb 451 {
vanshg 10:810d1849da9d 452 dipFlags |= BUTTON1_FLAG;
vanshg 11:8fc2b703086b 453 printDipFlag();
vanshg 10:810d1849da9d 454 }
sahilmgandhi 14:9e7bb03ddccb 455 void enableButton2()
sahilmgandhi 14:9e7bb03ddccb 456 {
vanshg 10:810d1849da9d 457 dipFlags |= BUTTON2_FLAG;
vanshg 11:8fc2b703086b 458 printDipFlag();
vanshg 10:810d1849da9d 459 }
sahilmgandhi 14:9e7bb03ddccb 460 void enableButton3()
sahilmgandhi 14:9e7bb03ddccb 461 {
vanshg 10:810d1849da9d 462 dipFlags |= BUTTON3_FLAG;
vanshg 11:8fc2b703086b 463 printDipFlag();
vanshg 10:810d1849da9d 464 }
sahilmgandhi 14:9e7bb03ddccb 465 void enableButton4()
sahilmgandhi 14:9e7bb03ddccb 466 {
vanshg 10:810d1849da9d 467 dipFlags |= BUTTON4_FLAG;
vanshg 11:8fc2b703086b 468 printDipFlag();
vanshg 10:810d1849da9d 469 }
sahilmgandhi 14:9e7bb03ddccb 470 void disableButton1()
sahilmgandhi 14:9e7bb03ddccb 471 {
vanshg 10:810d1849da9d 472 dipFlags &= ~BUTTON1_FLAG;
vanshg 11:8fc2b703086b 473 printDipFlag();
vanshg 10:810d1849da9d 474 }
sahilmgandhi 14:9e7bb03ddccb 475 void disableButton2()
sahilmgandhi 14:9e7bb03ddccb 476 {
vanshg 10:810d1849da9d 477 dipFlags &= ~BUTTON2_FLAG;
vanshg 11:8fc2b703086b 478 printDipFlag();
vanshg 10:810d1849da9d 479 }
sahilmgandhi 14:9e7bb03ddccb 480 void disableButton3()
sahilmgandhi 14:9e7bb03ddccb 481 {
vanshg 10:810d1849da9d 482 dipFlags &= ~BUTTON3_FLAG;
vanshg 11:8fc2b703086b 483 printDipFlag();
vanshg 10:810d1849da9d 484 }
sahilmgandhi 14:9e7bb03ddccb 485 void disableButton4()
sahilmgandhi 14:9e7bb03ddccb 486 {
vanshg 10:810d1849da9d 487 dipFlags &= ~BUTTON4_FLAG;
vanshg 11:8fc2b703086b 488 printDipFlag();
kyleliangus 9:1d8e4da058cd 489 }
christine222 3:880f15be8c72 490
kyleliangus 15:b80555a4a8b9 491 void pidOnEncoders()
kyleliangus 15:b80555a4a8b9 492 {
kyleliangus 15:b80555a4a8b9 493 int count0;
kyleliangus 15:b80555a4a8b9 494 int count1;
kyleliangus 15:b80555a4a8b9 495 count0 = encoder0.getPulses();
kyleliangus 15:b80555a4a8b9 496 count1 = encoder1.getPulses();
kyleliangus 15:b80555a4a8b9 497 int diff = count0 - count1;
sahilmgandhi 20:82836745332e 498 double kp = 0.00011;
sahilmgandhi 20:82836745332e 499 double kd = 0.00014;
kyleliangus 15:b80555a4a8b9 500 int prev = 0;
sahilmgandhi 16:d9252437bd92 501
sahilmgandhi 16:d9252437bd92 502 int counter = 0;
kyleliangus 15:b80555a4a8b9 503 while(1)
kyleliangus 15:b80555a4a8b9 504 {
kyleliangus 15:b80555a4a8b9 505 count0 = encoder0.getPulses();
kyleliangus 15:b80555a4a8b9 506 count1 = encoder1.getPulses();
kyleliangus 15:b80555a4a8b9 507 int x = count0 - count1;
kyleliangus 15:b80555a4a8b9 508 //double d = kp * x + kd * ( x - prev );
kyleliangus 15:b80555a4a8b9 509 double kppart = kp * x;
kyleliangus 15:b80555a4a8b9 510 double kdpart = kd * (x-prev);
kyleliangus 15:b80555a4a8b9 511 double d = kppart + kdpart;
kyleliangus 15:b80555a4a8b9 512
kyleliangus 15:b80555a4a8b9 513 //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 514 if( x < diff - 40 ) // count1 is bigger, right wheel pushed forward
kyleliangus 15:b80555a4a8b9 515 {
kyleliangus 15:b80555a4a8b9 516 left_motor.move( -d );
kyleliangus 15:b80555a4a8b9 517 right_motor.move( d );
kyleliangus 15:b80555a4a8b9 518 }
sahilmgandhi 20:82836745332e 519 else if( x > diff + 40 )
kyleliangus 15:b80555a4a8b9 520 {
kyleliangus 15:b80555a4a8b9 521 left_motor.move( -d );
kyleliangus 15:b80555a4a8b9 522 right_motor.move( d );
kyleliangus 15:b80555a4a8b9 523 }
sahilmgandhi 16:d9252437bd92 524 // else
sahilmgandhi 16:d9252437bd92 525 // {
sahilmgandhi 16:d9252437bd92 526 // left_motor.brake();
sahilmgandhi 16:d9252437bd92 527 // right_motor.brake();
sahilmgandhi 16:d9252437bd92 528 // }
kyleliangus 15:b80555a4a8b9 529 prev = x;
sahilmgandhi 16:d9252437bd92 530 counter++;
sahilmgandhi 19:7b66a518b6f8 531 if (counter == 5)
sahilmgandhi 16:d9252437bd92 532 break;
kyleliangus 15:b80555a4a8b9 533 }
kyleliangus 15:b80555a4a8b9 534 }
kyleliangus 15:b80555a4a8b9 535
sahilmgandhi 20:82836745332e 536 void oneCellEncoderAndIR(double cellCount){
sahilmgandhi 19:7b66a518b6f8 537 double currentError = 0;
sahilmgandhi 19:7b66a518b6f8 538 double previousError = 0;
sahilmgandhi 19:7b66a518b6f8 539 double derivError = 0;
sahilmgandhi 19:7b66a518b6f8 540 double sumError = 0;
sahilmgandhi 17:f713758f6238 541
sahilmgandhi 19:7b66a518b6f8 542 double HIGH_PWM_VOLTAGE = 0.1;
sahilmgandhi 19:7b66a518b6f8 543 double rightSpeed = 0.10;
sahilmgandhi 19:7b66a518b6f8 544 double leftSpeed = 0.10;
sahilmgandhi 19:7b66a518b6f8 545
sahilmgandhi 20:82836745332e 546 int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellCount;
sahilmgandhi 20:82836745332e 547 int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellCount;
sahilmgandhi 19:7b66a518b6f8 548
sahilmgandhi 20:82836745332e 549 left_motor.forward(0.125);
sahilmgandhi 19:7b66a518b6f8 550 right_motor.forward(0.10);
sahilmgandhi 19:7b66a518b6f8 551
sahilmgandhi 20:82836745332e 552 float receiverTwoReading = 0.0;
sahilmgandhi 20:82836745332e 553 float receiverThreeReading = 0.0;
sahilmgandhi 20:82836745332e 554
sahilmgandhi 19:7b66a518b6f8 555 float ir2 = IRP_2.getSamples( SAMPLE_NUM );
sahilmgandhi 19:7b66a518b6f8 556 float ir3 = IRP_3.getSamples( SAMPLE_NUM );
sahilmgandhi 19:7b66a518b6f8 557
sahilmgandhi 19:7b66a518b6f8 558 while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1){
sahilmgandhi 20:82836745332e 559 receiverTwoReading = IRP_2.getSamples(100);
sahilmgandhi 20:82836745332e 560 receiverThreeReading = IRP_3.getSamples(100);
sahilmgandhi 20:82836745332e 561 if (receiverTwoReading <= IRP_2.sensorAvg/4)
sahilmgandhi 20:82836745332e 562 currentError = receiverThreeReading - IRP_3.sensorAvg;
sahilmgandhi 20:82836745332e 563 else if (receiverThreeReading <= IRP_3.sensorAvg/4)
sahilmgandhi 20:82836745332e 564 currentError = receiverTwoReading - IRP_2.sensorAvg;
sahilmgandhi 20:82836745332e 565 // else if (receiverTwoReading <= IRP_2.sensorAvg/2 && receiverThreeReading <= IRP_3.sensorAvg/2) // scenario when both left and right wall missing, use encoders only
sahilmgandhi 20:82836745332e 566 // moveForwardCellEncoder(2);
sahilmgandhi 20:82836745332e 567 else
sahilmgandhi 20:82836745332e 568 currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg));
sahilmgandhi 20:82836745332e 569
sahilmgandhi 20:82836745332e 570 sumError += currentError;
sahilmgandhi 19:7b66a518b6f8 571 derivError = currentError - previousError;
sahilmgandhi 19:7b66a518b6f8 572 double PIDSum = IP_CONSTANT*currentError + II_CONSTANT*sumError + ID_CONSTANT*derivError;
sahilmgandhi 19:7b66a518b6f8 573 if (PIDSum > 0) { // this means the leftWheel is faster than the right. So right speeds up, left slows down
sahilmgandhi 19:7b66a518b6f8 574 rightSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE);
sahilmgandhi 19:7b66a518b6f8 575 leftSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE);
sahilmgandhi 19:7b66a518b6f8 576 } else { // r is faster than L. speed up l, slow down r
sahilmgandhi 19:7b66a518b6f8 577 rightSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE);
sahilmgandhi 19:7b66a518b6f8 578 leftSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE);
sahilmgandhi 19:7b66a518b6f8 579 }
sahilmgandhi 19:7b66a518b6f8 580 if (leftSpeed > 0.30) leftSpeed = 0.30;
sahilmgandhi 19:7b66a518b6f8 581 if (leftSpeed < 0) leftSpeed = 0;
sahilmgandhi 19:7b66a518b6f8 582 if (rightSpeed > 0.30) rightSpeed = 0.30;
sahilmgandhi 19:7b66a518b6f8 583 if (rightSpeed < 0) rightSpeed = 0;
sahilmgandhi 19:7b66a518b6f8 584
sahilmgandhi 19:7b66a518b6f8 585 right_motor.forward(rightSpeed);
sahilmgandhi 19:7b66a518b6f8 586 left_motor.forward(leftSpeed);
sahilmgandhi 19:7b66a518b6f8 587 pidOnEncoders();
sahilmgandhi 19:7b66a518b6f8 588
sahilmgandhi 19:7b66a518b6f8 589 previousError = currentError;
sahilmgandhi 19:7b66a518b6f8 590 ir2 = IRP_2.getSamples( SAMPLE_NUM );
sahilmgandhi 19:7b66a518b6f8 591 ir3 = IRP_3.getSamples( SAMPLE_NUM );
sahilmgandhi 19:7b66a518b6f8 592 }
sahilmgandhi 19:7b66a518b6f8 593
christine222 21:9a6cb07bdcb6 594
christine222 21:9a6cb07bdcb6 595
sahilmgandhi 19:7b66a518b6f8 596 left_motor.brake();
sahilmgandhi 19:7b66a518b6f8 597 right_motor.brake();
sahilmgandhi 17:f713758f6238 598 }
sahilmgandhi 17:f713758f6238 599
sahilmgandhi 0:a03c771ab78e 600 int main()
sahilmgandhi 0:a03c771ab78e 601 {
christine222 3:880f15be8c72 602 //Set highest bandwidth.
sahilmgandhi 1:8a4b2f573923 603 gyro.setLpBandwidth(LPFBW_42HZ);
christine222 3:880f15be8c72 604 serial.baud(9600);
sahilmgandhi 7:6f5cb6377bd4 605 serial.printf("The gyro's address is %s", gyro.getWhoAmI());
sahilmgandhi 7:6f5cb6377bd4 606
sahilmgandhi 1:8a4b2f573923 607 wait (0.1);
sahilmgandhi 7:6f5cb6377bd4 608
christine222 3:880f15be8c72 609
sahilmgandhi 2:771db996cee0 610 redLed.write(1);
sahilmgandhi 14:9e7bb03ddccb 611 greenLed.write(0);
sahilmgandhi 2:771db996cee0 612 blueLed.write(1);
sahilmgandhi 14:9e7bb03ddccb 613
kyleliangus 9:1d8e4da058cd 614 //left_motor.forward(0.1);
kyleliangus 9:1d8e4da058cd 615 //right_motor.forward(0.1);
kyleliangus 8:a0760acdc59e 616
kyleliangus 8:a0760acdc59e 617 // PA_1 is A of right
kyleliangus 8:a0760acdc59e 618 // PA_0 is B of right
kyleliangus 8:a0760acdc59e 619 // PA_5 is A of left
kyleliangus 8:a0760acdc59e 620 // PB_3 is B of left
vanshg 11:8fc2b703086b 621 //QEI encoder0( PA_5, PB_3, NC, PULSES, QEI::X4_ENCODING );
vanshg 11:8fc2b703086b 622 // QEI encoder1( PA_1, PA_0, NC, PULSES, QEI::X4_ENCODING );
sahilmgandhi 14:9e7bb03ddccb 623
vanshg 10:810d1849da9d 624 // TODO: Setting all the registers and what not for Quadrature Encoders
sahilmgandhi 14:9e7bb03ddccb 625 /* RCC->APB1ENR |= 0x1001; // Enable clock for Tim2 (Bit 0) and Tim5 (Bit 3)
sahilmgandhi 14:9e7bb03ddccb 626 RCC->AHB1ENR |= 0x11; // Enable GPIO port clock enables for Tim2(A) and Tim5(B)
sahilmgandhi 14:9e7bb03ddccb 627 GPIOA->AFR[0] |= 0x10; // Set GPIO alternate function modes for Tim2
sahilmgandhi 14:9e7bb03ddccb 628 GPIOB->AFR[0] |= 0x100; // Set GPIO alternate function modes for Tim5
sahilmgandhi 14:9e7bb03ddccb 629 */
sahilmgandhi 14:9e7bb03ddccb 630
kyleliangus 12:5790e56a056f 631 // 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 632 // of alternate function specified
kyleliangus 12:5790e56a056f 633 // 4 modes sets AHB1ENR
kyleliangus 12:5790e56a056f 634 // Now TMR: enable clock with timer, APB1ENR
kyleliangus 12:5790e56a056f 635 // set period, autoreload value, ARR value 2^32-1, CR1 - TMR resets itself, ARPE and EN
kyleliangus 12:5790e56a056f 636 //
kyleliangus 12:5790e56a056f 637 // Encoder mode: disable timer before changing timer to encoder
kyleliangus 12:5790e56a056f 638 // CCMR1/2 1/2 depends on channel 1/2 or 3/4, depends on upper bits, depending which channels you use
kyleliangus 12:5790e56a056f 639 // CCMR sets sample rate and set the channel to input
kyleliangus 12:5790e56a056f 640 // CCER, which edge to trigger on, cannot be 11(not allowed for encoder mode), CCER for both channels
kyleliangus 12:5790e56a056f 641 // SMCR - encoder mode
kyleliangus 12:5790e56a056f 642 // CR1 reenabales
kyleliangus 12:5790e56a056f 643 // then read CNT reg of timer
sahilmgandhi 14:9e7bb03ddccb 644
sahilmgandhi 14:9e7bb03ddccb 645
vanshg 10:810d1849da9d 646 dipButton1.rise(&enableButton1);
vanshg 10:810d1849da9d 647 dipButton2.rise(&enableButton2);
vanshg 10:810d1849da9d 648 dipButton3.rise(&enableButton3);
vanshg 10:810d1849da9d 649 dipButton4.rise(&enableButton4);
sahilmgandhi 14:9e7bb03ddccb 650
vanshg 10:810d1849da9d 651 dipButton1.fall(&disableButton1);
vanshg 10:810d1849da9d 652 dipButton2.fall(&disableButton2);
vanshg 10:810d1849da9d 653 dipButton3.fall(&disableButton3);
vanshg 10:810d1849da9d 654 dipButton4.fall(&disableButton4);
sahilmgandhi 7:6f5cb6377bd4 655
kyleliangus 15:b80555a4a8b9 656 //right_motor.forward( 0.2 );
kyleliangus 15:b80555a4a8b9 657 //left_motor.forward( 0.2 );
kyleliangus 15:b80555a4a8b9 658
christine222 3:880f15be8c72 659 while (1) {
christine222 21:9a6cb07bdcb6 660
christine222 21:9a6cb07bdcb6 661 //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ));
christine222 21:9a6cb07bdcb6 662
sahilmgandhi 20:82836745332e 663 // oneCellEncoderAndIR(5);
christine222 21:9a6cb07bdcb6 664 moveForwardUntilWallIr();
christine222 21:9a6cb07bdcb6 665 //moveForwardEncoder();
christine222 21:9a6cb07bdcb6 666 //serial.printf("ded \n");
christine222 21:9a6cb07bdcb6 667 //break;
christine222 21:9a6cb07bdcb6 668
christine222 21:9a6cb07bdcb6 669
christine222 21:9a6cb07bdcb6 670 //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ));
christine222 21:9a6cb07bdcb6 671
christine222 21:9a6cb07bdcb6 672
christine222 21:9a6cb07bdcb6 673 //break;
christine222 21:9a6cb07bdcb6 674 // moveForwardCellEncoder(1);
christine222 21:9a6cb07bdcb6 675 // wait(0.5);
christine222 21:9a6cb07bdcb6 676 // handleTurns();
christine222 21:9a6cb07bdcb6 677 // wait(0.5);
christine222 21:9a6cb07bdcb6 678 // moveForwardCellEncoder(1);
christine222 21:9a6cb07bdcb6 679 // wait(0.5);
christine222 21:9a6cb07bdcb6 680 // handleTurns();
christine222 21:9a6cb07bdcb6 681 //break;
sahilmgandhi 16:d9252437bd92 682 //pidOnEncoders();
sahilmgandhi 20:82836745332e 683 // moveForwardUntilWallIr();
kyleliangus 8:a0760acdc59e 684 //serial.printf("%i, %i, %i\n", gyro.getGyroX(), gyro.getGyroY(), gyro.getGyroZ());
kyleliangus 15:b80555a4a8b9 685 //serial.printf("Pulse Count=> e0:%d, e1:%d \r\n", encoder0.getPulses(),encoder1.getPulses());
sahilmgandhi 17:f713758f6238 686 // double currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ;
christine222 21:9a6cb07bdcb6 687 //serial.printf("IRS= >: %f, %f, %f, %f, %f \r\n", IRP_1.getSamples( 100 ), IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ), IRP_4.getSamples(100), currentError );
sahilmgandhi 14:9e7bb03ddccb 688
christine222 3:880f15be8c72 689 //reading = Rec_4.read();
christine222 3:880f15be8c72 690 // serial.printf("reading: %f\n", reading);
christine222 3:880f15be8c72 691 }
sahilmgandhi 0:a03c771ab78e 692 }