MacroRat / MouseCode

Mouse code for the MacroRat

Dependencies:   ITG3200 QEI

main.cpp@16:d9252437bd92, 2017-05-14 (annotated)

Committer:
sahilmgandhi
Date:
Sun May 14 20:58:55 2017 +0000
Revision:
16:d9252437bd92
Parent:
15:b80555a4a8b9
Child:
17:f713758f6238
Moves forward exactly one cell now!;

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