MacroRat / MouseCode

Mouse code for the MacroRat

Dependencies:   ITG3200 QEI

main.cpp@13:2032db00f168, 2017-05-13 (annotated)

Committer:
christine222
Date:
Sat May 13 02:40:49 2017 +0000
Revision:
13:2032db00f168
Parent:
12:5790e56a056f
Child:
14:9e7bb03ddccb
turning sort of works

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"
vanshg 10:810d1849da9d 9 #include "stm32f4xx.h"
sahilmgandhi 7:6f5cb6377bd4 10 #include "QEI.h"
sahilmgandhi 0:a03c771ab78e 11
kyleliangus 9:1d8e4da058cd 12
vanshg 11:8fc2b703086b 13 /* Constants for when HIGH_PWM_VOLTAGE = 0.2
kyleliangus 9:1d8e4da058cd 14 #define IP_CONSTANT 6
kyleliangus 9:1d8e4da058cd 15 #define II_CONSTANT 0
kyleliangus 9:1d8e4da058cd 16 #define ID_CONSTANT 1
vanshg 11:8fc2b703086b 17 */
sahilmgandhi 0:a03c771ab78e 18
vanshg 11:8fc2b703086b 19 // Constants for when HIGH_PWM_VOLTAGE = 0.1
christine222 13:2032db00f168 20 #define IP_CONSTANT 13
vanshg 11:8fc2b703086b 21 #define II_CONSTANT 0
christine222 13:2032db00f168 22 #define ID_CONSTANT 1.
christine222 13:2032db00f168 23
christine222 13:2032db00f168 24 const int desiredCountR = 1300;
christine222 13:2032db00f168 25 const int desiredCountL = 1400;
christine222 13:2032db00f168 26
christine222 13:2032db00f168 27 void turnLeft(){
christine222 13:2032db00f168 28 double speed0 = 0.15;
christine222 13:2032db00f168 29 double speed1 = 0.15;
christine222 13:2032db00f168 30 double kp = 0.01;
christine222 13:2032db00f168 31
christine222 13:2032db00f168 32 int counter = 0;
christine222 13:2032db00f168 33
christine222 13:2032db00f168 34 int initialCount0 = encoder0.getPulses();
christine222 13:2032db00f168 35 int initialCount1 = encoder1.getPulses();
christine222 13:2032db00f168 36
christine222 13:2032db00f168 37 double desiredCount0 = initialCount0 - desiredCountL;
christine222 13:2032db00f168 38 double desiredCount1 = initialCount1 + desiredCountL;
christine222 13:2032db00f168 39
christine222 13:2032db00f168 40 int count0 = initialCount0;
christine222 13:2032db00f168 41 int count1 = initialCount1;
christine222 13:2032db00f168 42
christine222 13:2032db00f168 43 double error0 = count0 - desiredCount0;
christine222 13:2032db00f168 44 double error1 = count1 - desiredCount1;
christine222 13:2032db00f168 45
christine222 13:2032db00f168 46 while(1){
christine222 13:2032db00f168 47
christine222 13:2032db00f168 48 if(!(abs(error0) < 1) && !(abs(error1) < 1)){
christine222 13:2032db00f168 49 count0 = encoder0.getPulses();
christine222 13:2032db00f168 50 count1 = encoder1.getPulses();
christine222 13:2032db00f168 51
christine222 13:2032db00f168 52 error0 = count0 - desiredCount0;
christine222 13:2032db00f168 53 error1 = count1 - desiredCount1;
christine222 13:2032db00f168 54
christine222 13:2032db00f168 55 speed0 = error0 * kp + speed0;
christine222 13:2032db00f168 56 speed1 = error1 * kp + speed1;
christine222 13:2032db00f168 57
christine222 13:2032db00f168 58 right_motor.move(speed0);
christine222 13:2032db00f168 59 left_motor.move(speed1);
christine222 13:2032db00f168 60 counter = 0;
christine222 13:2032db00f168 61 }else{
christine222 13:2032db00f168 62 counter++;
christine222 13:2032db00f168 63 right_motor.brake();
christine222 13:2032db00f168 64 left_motor.brake();
christine222 13:2032db00f168 65 }
christine222 13:2032db00f168 66
christine222 13:2032db00f168 67 if (counter > 60){
christine222 13:2032db00f168 68 break;
christine222 13:2032db00f168 69 }
christine222 13:2032db00f168 70 }
christine222 13:2032db00f168 71
christine222 13:2032db00f168 72 right_motor.brake();
christine222 13:2032db00f168 73 left_motor.brake();
christine222 13:2032db00f168 74 }
kyleliangus 9:1d8e4da058cd 75
kyleliangus 12:5790e56a056f 76 void turnRight() { //e1 should be negative encoder0 is left, encoder1 is right
kyleliangus 12:5790e56a056f 77 double speed0 = 0.15;
kyleliangus 12:5790e56a056f 78 double speed1 = 0.15;
christine222 13:2032db00f168 79 double kp = 0.01;
christine222 13:2032db00f168 80
christine222 13:2032db00f168 81 int counter = 0;
christine222 13:2032db00f168 82
christine222 13:2032db00f168 83 int initialCount0 = encoder0.getPulses();
christine222 13:2032db00f168 84 int initialCount1 = encoder1.getPulses();
christine222 13:2032db00f168 85
christine222 13:2032db00f168 86 double desiredCount0 = initialCount0 + desiredCountR;
christine222 13:2032db00f168 87 double desiredCount1 = initialCount1 - desiredCountR;
christine222 13:2032db00f168 88
christine222 13:2032db00f168 89 int count0 = initialCount0;
christine222 13:2032db00f168 90 int count1 = initialCount1;
kyleliangus 12:5790e56a056f 91
christine222 13:2032db00f168 92 double error0 = count0 - desiredCount0;
christine222 13:2032db00f168 93 double error1 = count1 - desiredCount1;
christine222 13:2032db00f168 94
christine222 13:2032db00f168 95 while(1){
christine222 13:2032db00f168 96
christine222 13:2032db00f168 97 if(!(abs(error0) < 1) && !(abs(error1) < 1)){
christine222 13:2032db00f168 98 count0 = encoder0.getPulses();
christine222 13:2032db00f168 99 count1 = encoder1.getPulses();
christine222 13:2032db00f168 100
christine222 13:2032db00f168 101 error0 = count0 - desiredCount0; // moves forward
christine222 13:2032db00f168 102 error1 = count1 - desiredCount1; // moves backwards
christine222 13:2032db00f168 103
christine222 13:2032db00f168 104 speed0 = error0 * kp + speed0;
christine222 13:2032db00f168 105 speed1 = error1 * kp + speed1;
kyleliangus 12:5790e56a056f 106
christine222 13:2032db00f168 107 right_motor.move(speed0);
christine222 13:2032db00f168 108 left_motor.move(speed1);
christine222 13:2032db00f168 109 counter = 0;
christine222 13:2032db00f168 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 }
kyleliangus 12:5790e56a056f 115
christine222 13:2032db00f168 116 if (counter > 60){
christine222 13:2032db00f168 117 break;
christine222 13:2032db00f168 118 }
christine222 13:2032db00f168 119
christine222 13:2032db00f168 120 // serial.printf("ERROR=> 0:%f, 1:%f, SPEED=> 0:%f, 1:%f\n", error0, error1, speed0, speed1);
christine222 13:2032db00f168 121 // serial.printf("Pulse Count=> e0:%d, e1:%d \r\n", encoder0.getPulses(), encoder1.getPulses());
christine222 13:2032db00f168 122 }
christine222 13:2032db00f168 123
christine222 13:2032db00f168 124 right_motor.brake();
christine222 13:2032db00f168 125 left_motor.brake();
christine222 13:2032db00f168 126 }
christine222 13:2032db00f168 127
christine222 13:2032db00f168 128 void turnLeft180(){
christine222 13:2032db00f168 129 double speed0 = 0.15;
christine222 13:2032db00f168 130 double speed1 = 0.15;
christine222 13:2032db00f168 131 double kp = 0.01;
christine222 13:2032db00f168 132
christine222 13:2032db00f168 133 int counter = 0;
kyleliangus 12:5790e56a056f 134
kyleliangus 12:5790e56a056f 135 int initialCount0 = encoder0.getPulses();
kyleliangus 12:5790e56a056f 136 int initialCount1 = encoder1.getPulses();
kyleliangus 12:5790e56a056f 137
christine222 13:2032db00f168 138 double desiredCount0 = initialCount0 - 3000;
christine222 13:2032db00f168 139 double desiredCount1 = initialCount1 + 2700;
christine222 13:2032db00f168 140
christine222 13:2032db00f168 141 int count0 = initialCount0;
christine222 13:2032db00f168 142 int count1 = initialCount1;
christine222 13:2032db00f168 143
christine222 13:2032db00f168 144 double error0 = count0 - desiredCount0;
christine222 13:2032db00f168 145 double error1 = count1 - desiredCount1;
christine222 13:2032db00f168 146
christine222 13:2032db00f168 147 while(1){
christine222 13:2032db00f168 148
christine222 13:2032db00f168 149 if(!(abs(error0) < 1) && !(abs(error1) < 1)){
christine222 13:2032db00f168 150 count0 = encoder0.getPulses();
christine222 13:2032db00f168 151 count1 = encoder1.getPulses();
christine222 13:2032db00f168 152
christine222 13:2032db00f168 153 error0 = count0 - desiredCount0;
christine222 13:2032db00f168 154 error1 = count1 - desiredCount1;
kyleliangus 12:5790e56a056f 155
christine222 13:2032db00f168 156 speed0 = error0 * kp + speed0;
christine222 13:2032db00f168 157 speed1 = error1 * kp + speed1;
kyleliangus 12:5790e56a056f 158
christine222 13:2032db00f168 159 right_motor.move(speed0);
christine222 13:2032db00f168 160 left_motor.move(speed1);
christine222 13:2032db00f168 161 counter = 0;
christine222 13:2032db00f168 162 }else{
christine222 13:2032db00f168 163 counter++;
christine222 13:2032db00f168 164 right_motor.brake();
christine222 13:2032db00f168 165 left_motor.brake();
christine222 13:2032db00f168 166 }
christine222 13:2032db00f168 167
christine222 13:2032db00f168 168 if (counter > 60){
christine222 13:2032db00f168 169 break;
christine222 13:2032db00f168 170 }
christine222 13:2032db00f168 171 }
christine222 13:2032db00f168 172
christine222 13:2032db00f168 173 right_motor.brake();
christine222 13:2032db00f168 174 left_motor.brake();
christine222 13:2032db00f168 175 }
kyleliangus 12:5790e56a056f 176
christine222 13:2032db00f168 177 void turnRight180(){
christine222 13:2032db00f168 178 double speed0 = 0.15;
christine222 13:2032db00f168 179 double speed1 = 0.15;
christine222 13:2032db00f168 180 double kp = 0.01;
christine222 13:2032db00f168 181
christine222 13:2032db00f168 182 int counter = 0;
christine222 13:2032db00f168 183
christine222 13:2032db00f168 184 int initialCount0 = encoder0.getPulses();
christine222 13:2032db00f168 185 int initialCount1 = encoder1.getPulses();
kyleliangus 12:5790e56a056f 186
christine222 13:2032db00f168 187 double desiredCount0 = initialCount0 + desiredCountR*2;
christine222 13:2032db00f168 188 double desiredCount1 = initialCount1 - desiredCountR*2;
christine222 13:2032db00f168 189
christine222 13:2032db00f168 190 int count0 = initialCount0;
christine222 13:2032db00f168 191 int count1 = initialCount1;
christine222 13:2032db00f168 192
christine222 13:2032db00f168 193 double error0 = count0 - desiredCount0;
christine222 13:2032db00f168 194 double error1 = count1 - desiredCount1;
christine222 13:2032db00f168 195
christine222 13:2032db00f168 196 while(1){
kyleliangus 12:5790e56a056f 197
christine222 13:2032db00f168 198 if(!(abs(error0) < 1) && !(abs(error1) < 1)){
christine222 13:2032db00f168 199 count0 = encoder0.getPulses();
christine222 13:2032db00f168 200 count1 = encoder1.getPulses();
christine222 13:2032db00f168 201
christine222 13:2032db00f168 202 error0 = count0 - desiredCount0; // moves forward
christine222 13:2032db00f168 203 error1 = count1 - desiredCount1; // moves backwards
christine222 13:2032db00f168 204
christine222 13:2032db00f168 205 speed0 = error0 * kp + speed0;
christine222 13:2032db00f168 206 speed1 = error1 * kp + speed1;
kyleliangus 12:5790e56a056f 207
christine222 13:2032db00f168 208 right_motor.move(speed0);
christine222 13:2032db00f168 209 left_motor.move(speed1);
christine222 13:2032db00f168 210 counter = 0;
christine222 13:2032db00f168 211 }else{
christine222 13:2032db00f168 212 counter++;
christine222 13:2032db00f168 213 right_motor.brake();
christine222 13:2032db00f168 214 left_motor.brake();
christine222 13:2032db00f168 215 }
christine222 13:2032db00f168 216
christine222 13:2032db00f168 217 if (counter > 60){
christine222 13:2032db00f168 218 break;
christine222 13:2032db00f168 219 }
christine222 13:2032db00f168 220
christine222 13:2032db00f168 221 // serial.printf("ERROR=> 0:%f, 1:%f, SPEED=> 0:%f, 1:%f\n", error0, error1, speed0, speed1);
christine222 13:2032db00f168 222 // serial.printf("Pulse Count=> e0:%d, e1:%d \r\n", encoder0.getPulses(), encoder1.getPulses());
kyleliangus 12:5790e56a056f 223 }
kyleliangus 12:5790e56a056f 224
kyleliangus 12:5790e56a056f 225 right_motor.brake();
kyleliangus 12:5790e56a056f 226 left_motor.brake();
kyleliangus 12:5790e56a056f 227 }
kyleliangus 12:5790e56a056f 228
kyleliangus 9:1d8e4da058cd 229 void moveForwardUntilWallIr() {
kyleliangus 9:1d8e4da058cd 230 double currentError = 0;
kyleliangus 9:1d8e4da058cd 231 double previousError = 0;
kyleliangus 9:1d8e4da058cd 232 double derivError = 0;
kyleliangus 9:1d8e4da058cd 233 double sumError = 0;
kyleliangus 9:1d8e4da058cd 234
vanshg 11:8fc2b703086b 235 double HIGH_PWM_VOLTAGE = 0.1;
kyleliangus 9:1d8e4da058cd 236
vanshg 11:8fc2b703086b 237 double rightSpeed = 0.1;
vanshg 11:8fc2b703086b 238 double leftSpeed = 0.1;
kyleliangus 9:1d8e4da058cd 239
kyleliangus 9:1d8e4da058cd 240 float ir2 = IRP_2.getSamples( SAMPLE_NUM );
kyleliangus 9:1d8e4da058cd 241 float ir3 = IRP_3.getSamples( SAMPLE_NUM );
kyleliangus 9:1d8e4da058cd 242
vanshg 11:8fc2b703086b 243 int count = encoder0.getPulses();
vanshg 11:8fc2b703086b 244 while ((IRP_1.getSamples( SAMPLE_NUM ) + IRP_4.getSamples( SAMPLE_NUM ) )/2 < 0.05f) {
vanshg 11:8fc2b703086b 245 int pulseCount = (encoder0.getPulses()-count) % 5600;
vanshg 11:8fc2b703086b 246 if (pulseCount > 5400 && pulseCount < 5800) {
vanshg 11:8fc2b703086b 247 blueLed.write(0);
vanshg 11:8fc2b703086b 248 } else {
vanshg 11:8fc2b703086b 249 blueLed.write(1);
vanshg 11:8fc2b703086b 250 }
vanshg 11:8fc2b703086b 251
vanshg 11:8fc2b703086b 252
kyleliangus 9:1d8e4da058cd 253 currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ;
kyleliangus 9:1d8e4da058cd 254 derivError = currentError - previousError;
kyleliangus 9:1d8e4da058cd 255 double PIDSum = IP_CONSTANT*currentError + II_CONSTANT*sumError + ID_CONSTANT*derivError;
kyleliangus 9:1d8e4da058cd 256 if (PIDSum > 0) { // this means the leftWheel is faster than the right. So right speeds up, left slows down
kyleliangus 9:1d8e4da058cd 257 rightSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE);
kyleliangus 9:1d8e4da058cd 258 leftSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE);
kyleliangus 9:1d8e4da058cd 259 } else { // r is faster than L. speed up l, slow down r
kyleliangus 9:1d8e4da058cd 260 rightSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE);
kyleliangus 9:1d8e4da058cd 261 leftSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE);
kyleliangus 9:1d8e4da058cd 262 }
kyleliangus 9:1d8e4da058cd 263
kyleliangus 9:1d8e4da058cd 264 if (leftSpeed > 0.30) leftSpeed = 0.30;
kyleliangus 9:1d8e4da058cd 265 if (leftSpeed < 0) leftSpeed = 0;
kyleliangus 9:1d8e4da058cd 266 if (rightSpeed > 0.30) rightSpeed = 0.30;
kyleliangus 9:1d8e4da058cd 267 if (rightSpeed < 0) rightSpeed = 0;
kyleliangus 9:1d8e4da058cd 268
kyleliangus 9:1d8e4da058cd 269 right_motor.forward(rightSpeed);
kyleliangus 9:1d8e4da058cd 270 left_motor.forward(leftSpeed);
kyleliangus 9:1d8e4da058cd 271
kyleliangus 9:1d8e4da058cd 272 previousError = currentError;
kyleliangus 9:1d8e4da058cd 273
kyleliangus 9:1d8e4da058cd 274 ir2 = IRP_2.getSamples( SAMPLE_NUM );
kyleliangus 9:1d8e4da058cd 275 ir3 = IRP_3.getSamples( SAMPLE_NUM );
kyleliangus 9:1d8e4da058cd 276
kyleliangus 9:1d8e4da058cd 277 }
kyleliangus 9:1d8e4da058cd 278 //sensor1Turn = IR_Sensor1.read();
kyleliangus 9:1d8e4da058cd 279 //sensor4Turn = IR_Sensor4.read();
kyleliangus 9:1d8e4da058cd 280
kyleliangus 9:1d8e4da058cd 281 //backward();
kyleliangus 9:1d8e4da058cd 282 wait_ms(40);
kyleliangus 9:1d8e4da058cd 283 //brake();
kyleliangus 9:1d8e4da058cd 284
kyleliangus 9:1d8e4da058cd 285 left_motor.brake();
kyleliangus 9:1d8e4da058cd 286 right_motor.brake();
vanshg 10:810d1849da9d 287 }
vanshg 10:810d1849da9d 288
vanshg 11:8fc2b703086b 289 void printDipFlag() {
vanshg 11:8fc2b703086b 290 if (DEBUGGING) serial.printf("Flag value is %d", dipFlags);
vanshg 11:8fc2b703086b 291 }
vanshg 11:8fc2b703086b 292
vanshg 10:810d1849da9d 293 void enableButton1() {
vanshg 10:810d1849da9d 294 dipFlags |= BUTTON1_FLAG;
vanshg 11:8fc2b703086b 295 printDipFlag();
vanshg 10:810d1849da9d 296 }
vanshg 10:810d1849da9d 297 void enableButton2() {
vanshg 10:810d1849da9d 298 dipFlags |= BUTTON2_FLAG;
vanshg 11:8fc2b703086b 299 printDipFlag();
vanshg 10:810d1849da9d 300 }
vanshg 10:810d1849da9d 301 void enableButton3() {
vanshg 10:810d1849da9d 302 dipFlags |= BUTTON3_FLAG;
vanshg 11:8fc2b703086b 303 printDipFlag();
vanshg 10:810d1849da9d 304 }
vanshg 10:810d1849da9d 305 void enableButton4() {
vanshg 10:810d1849da9d 306 dipFlags |= BUTTON4_FLAG;
vanshg 11:8fc2b703086b 307 printDipFlag();
vanshg 10:810d1849da9d 308 }
vanshg 10:810d1849da9d 309 void disableButton1() {
vanshg 10:810d1849da9d 310 dipFlags &= ~BUTTON1_FLAG;
vanshg 11:8fc2b703086b 311 printDipFlag();
vanshg 10:810d1849da9d 312 }
vanshg 10:810d1849da9d 313 void disableButton2() {
vanshg 10:810d1849da9d 314 dipFlags &= ~BUTTON2_FLAG;
vanshg 11:8fc2b703086b 315 printDipFlag();
vanshg 10:810d1849da9d 316 }
vanshg 10:810d1849da9d 317 void disableButton3() {
vanshg 10:810d1849da9d 318 dipFlags &= ~BUTTON3_FLAG;
vanshg 11:8fc2b703086b 319 printDipFlag();
vanshg 10:810d1849da9d 320 }
vanshg 10:810d1849da9d 321 void disableButton4() {
vanshg 10:810d1849da9d 322 dipFlags &= ~BUTTON4_FLAG;
vanshg 11:8fc2b703086b 323 printDipFlag();
kyleliangus 9:1d8e4da058cd 324 }
christine222 3:880f15be8c72 325
sahilmgandhi 0:a03c771ab78e 326 int main()
sahilmgandhi 0:a03c771ab78e 327 {
kyleliangus 8:a0760acdc59e 328 //enableMotors();
christine222 3:880f15be8c72 329 //Set highest bandwidth.
sahilmgandhi 1:8a4b2f573923 330 gyro.setLpBandwidth(LPFBW_42HZ);
christine222 3:880f15be8c72 331 serial.baud(9600);
sahilmgandhi 7:6f5cb6377bd4 332 serial.printf("The gyro's address is %s", gyro.getWhoAmI());
sahilmgandhi 7:6f5cb6377bd4 333
sahilmgandhi 1:8a4b2f573923 334 wait (0.1);
sahilmgandhi 7:6f5cb6377bd4 335
christine222 3:880f15be8c72 336 // IR_1.write(1);
sahilmgandhi 2:771db996cee0 337 // IR_2.write(1);
sahilmgandhi 2:771db996cee0 338 // IR_3.write(1);
sahilmgandhi 2:771db996cee0 339 // IR_4.write(1);
christine222 3:880f15be8c72 340
sahilmgandhi 2:771db996cee0 341 redLed.write(1);
vanshg 11:8fc2b703086b 342 greenLed.write(1);
sahilmgandhi 2:771db996cee0 343 blueLed.write(1);
sahilmgandhi 1:8a4b2f573923 344
kyleliangus 9:1d8e4da058cd 345 //left_motor.forward(0.1);
kyleliangus 9:1d8e4da058cd 346 //right_motor.forward(0.1);
kyleliangus 8:a0760acdc59e 347
kyleliangus 8:a0760acdc59e 348 // PA_1 is A of right
kyleliangus 8:a0760acdc59e 349 // PA_0 is B of right
kyleliangus 8:a0760acdc59e 350 // PA_5 is A of left
kyleliangus 8:a0760acdc59e 351 // PB_3 is B of left
vanshg 11:8fc2b703086b 352 //QEI encoder0( PA_5, PB_3, NC, PULSES, QEI::X4_ENCODING );
vanshg 11:8fc2b703086b 353 // QEI encoder1( PA_1, PA_0, NC, PULSES, QEI::X4_ENCODING );
vanshg 10:810d1849da9d 354
vanshg 10:810d1849da9d 355 // TODO: Setting all the registers and what not for Quadrature Encoders
christine222 13:2032db00f168 356 /* RCC->APB1ENR |= 0x1001; // Enable clock for Tim2 (Bit 0) and Tim5 (Bit 3)
vanshg 10:810d1849da9d 357 RCC->AHB1ENR |= 0x11; // Enable GPIO port clock enables for Tim2(A) and Tim5(B)
vanshg 10:810d1849da9d 358 GPIOA->AFR[0] |= 0x10; // Set GPIO alternate function modes for Tim2
vanshg 10:810d1849da9d 359 GPIOB->AFR[0] |= 0x100; // Set GPIO alternate function modes for Tim5
christine222 13:2032db00f168 360 */
vanshg 10:810d1849da9d 361
kyleliangus 12:5790e56a056f 362 // 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 363 // of alternate function specified
kyleliangus 12:5790e56a056f 364 // 4 modes sets AHB1ENR
kyleliangus 12:5790e56a056f 365 // Now TMR: enable clock with timer, APB1ENR
kyleliangus 12:5790e56a056f 366 // set period, autoreload value, ARR value 2^32-1, CR1 - TMR resets itself, ARPE and EN
kyleliangus 12:5790e56a056f 367 //
kyleliangus 12:5790e56a056f 368 // Encoder mode: disable timer before changing timer to encoder
kyleliangus 12:5790e56a056f 369 // CCMR1/2 1/2 depends on channel 1/2 or 3/4, depends on upper bits, depending which channels you use
kyleliangus 12:5790e56a056f 370 // CCMR sets sample rate and set the channel to input
kyleliangus 12:5790e56a056f 371 // CCER, which edge to trigger on, cannot be 11(not allowed for encoder mode), CCER for both channels
kyleliangus 12:5790e56a056f 372 // SMCR - encoder mode
kyleliangus 12:5790e56a056f 373 // CR1 reenabales
kyleliangus 12:5790e56a056f 374 // then read CNT reg of timer
kyleliangus 12:5790e56a056f 375
kyleliangus 12:5790e56a056f 376
vanshg 10:810d1849da9d 377 dipButton1.rise(&enableButton1);
vanshg 10:810d1849da9d 378 dipButton2.rise(&enableButton2);
vanshg 10:810d1849da9d 379 dipButton3.rise(&enableButton3);
vanshg 10:810d1849da9d 380 dipButton4.rise(&enableButton4);
vanshg 10:810d1849da9d 381
vanshg 10:810d1849da9d 382 dipButton1.fall(&disableButton1);
vanshg 10:810d1849da9d 383 dipButton2.fall(&disableButton2);
vanshg 10:810d1849da9d 384 dipButton3.fall(&disableButton3);
vanshg 10:810d1849da9d 385 dipButton4.fall(&disableButton4);
vanshg 10:810d1849da9d 386
sahilmgandhi 7:6f5cb6377bd4 387
christine222 3:880f15be8c72 388 while (1) {
vanshg 11:8fc2b703086b 389
christine222 13:2032db00f168 390 // moveForwardUntilWallIr();
christine222 13:2032db00f168 391 // wait(2);
christine222 13:2032db00f168 392 //turnRight180();
christine222 13:2032db00f168 393 wait(0.5);
christine222 13:2032db00f168 394 turnLeft180();
christine222 13:2032db00f168 395 wait(1);
christine222 13:2032db00f168 396 //turnRight180();
kyleliangus 12:5790e56a056f 397 wait(1);
christine222 13:2032db00f168 398 //turnLeft();
christine222 13:2032db00f168 399
christine222 13:2032db00f168 400 // wait(1);
christine222 13:2032db00f168 401 // turnLeft();
christine222 13:2032db00f168 402 // wait(1);
christine222 13:2032db00f168 403 // turnRight();
christine222 13:2032db00f168 404 // wait(1);
christine222 13:2032db00f168 405 // turnRight();
christine222 13:2032db00f168 406 // turnRight180();
christine222 13:2032db00f168 407 // wait(1);
christine222 13:2032db00f168 408 // turnLeft180();
christine222 13:2032db00f168 409 // wait(1);
christine222 13:2032db00f168 410 break;
kyleliangus 8:a0760acdc59e 411 //serial.printf("%i, %i, %i\n", gyro.getGyroX(), gyro.getGyroY(), gyro.getGyroZ());
vanshg 11:8fc2b703086b 412 // serial.printf("Pulse Count=> e0:%d, e1:%d \r\n", encoder0.getPulses(),encoder1.getPulses());
kyleliangus 9:1d8e4da058cd 413 //double currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ;
kyleliangus 9:1d8e4da058cd 414
kyleliangus 9:1d8e4da058cd 415 //serial.printf("IRS= >: %f, %f, %f, %f, %f \r\n",
kyleliangus 9:1d8e4da058cd 416 // IRP_1.getSamples( 100 ), IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ), IRP_4.getSamples(100), currentError );
kyleliangus 9:1d8e4da058cd 417
christine222 3:880f15be8c72 418 //reading = Rec_4.read();
christine222 3:880f15be8c72 419 // serial.printf("reading: %f\n", reading);
sahilmgandhi 1:8a4b2f573923 420 // redLed.write(0);
sahilmgandhi 1:8a4b2f573923 421 // wait_ms(1000);
sahilmgandhi 1:8a4b2f573923 422 // redLed.write(1);
sahilmgandhi 1:8a4b2f573923 423 // greenLed.write(0);
sahilmgandhi 1:8a4b2f573923 424 // wait_ms(1000);
sahilmgandhi 1:8a4b2f573923 425 // greenLed.write(1);
sahilmgandhi 1:8a4b2f573923 426 // blueLed.write(0);
sahilmgandhi 1:8a4b2f573923 427 // wait_ms(1000);
sahilmgandhi 1:8a4b2f573923 428 // blueLed.write(1);
christine222 3:880f15be8c72 429 }
sahilmgandhi 0:a03c771ab78e 430 }