MacroRat / MouseCode

Mouse code for the MacroRat

Dependencies:   ITG3200 QEI

main.cpp@22:681190ff98f0, 2017-05-19 (annotated)

Committer:
christine222
Date:
Fri May 19 01:15:36 2017 +0000
Revision:
22:681190ff98f0
Parent:
21:9a6cb07bdcb6
Child:
23:690b0ca34ee9
updated nCellEncoderandIR function

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