MacroRat / MouseCode

Mouse code for the MacroRat

Dependencies:   ITG3200 QEI

main.cpp@17:f713758f6238, 2017-05-14 (annotated)

Committer:
sahilmgandhi
Date:
Sun May 14 23:07:23 2017 +0000
Revision:
17:f713758f6238
Parent:
16:d9252437bd92
Child:
18:6a4db94011d3
Added mbed-dev and still working on dynamically deciding to turn;

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