MacroRat / MouseCode

Mouse code for the MacroRat

Dependencies:   ITG3200 QEI

main.cpp@20:82836745332e, 2017-05-17 (annotated)

Committer:
sahilmgandhi
Date:
Wed May 17 02:01:34 2017 +0000
Revision:
20:82836745332e
Parent:
19:7b66a518b6f8
Child:
21:9a6cb07bdcb6
added some init code to keep track of walls as moving. Turning works now (avg/4 gives good values so long as we are in the center)!; Also maybe we should start off facing back, and then turn 180 to get better average readings?

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