Mouse code for the MacroRat

Dependencies:   ITG3200 QEI

Committer:
kyleliangus
Date:
Fri May 12 23:25:07 2017 +0000
Revision:
12:5790e56a056f
Parent:
11:8fc2b703086b
Child:
13:2032db00f168
Right Turn is wrong, need fix

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
vanshg 11:8fc2b703086b 20 #define IP_CONSTANT 15
vanshg 11:8fc2b703086b 21 #define II_CONSTANT 0
vanshg 11:8fc2b703086b 22 #define ID_CONSTANT 0
kyleliangus 9:1d8e4da058cd 23
kyleliangus 12:5790e56a056f 24 void turnRight() { //e1 should be negative encoder0 is left, encoder1 is right
kyleliangus 12:5790e56a056f 25 double speed0 = 0.15;
kyleliangus 12:5790e56a056f 26 double speed1 = 0.15;
kyleliangus 12:5790e56a056f 27 double kp = 0.00001;
kyleliangus 12:5790e56a056f 28
kyleliangus 12:5790e56a056f 29 double desiredCount0 = 1500;
kyleliangus 12:5790e56a056f 30 double desiredCount1 = -1500;
kyleliangus 12:5790e56a056f 31
kyleliangus 12:5790e56a056f 32 double error0 = 10.1;
kyleliangus 12:5790e56a056f 33 double error1 = 10.1;
kyleliangus 12:5790e56a056f 34
kyleliangus 12:5790e56a056f 35 //right_motor.move(speed0);
kyleliangus 12:5790e56a056f 36 //left_motor.move(speed1);
kyleliangus 12:5790e56a056f 37
kyleliangus 12:5790e56a056f 38 int initialCount0 = encoder0.getPulses();
kyleliangus 12:5790e56a056f 39 int initialCount1 = encoder1.getPulses();
kyleliangus 12:5790e56a056f 40
kyleliangus 12:5790e56a056f 41 int count0 = encoder0.getPulses();
kyleliangus 12:5790e56a056f 42 int count1 = encoder1.getPulses();
kyleliangus 12:5790e56a056f 43
kyleliangus 12:5790e56a056f 44 while(!(abs(error0) < 10) && !(abs(error1) < 10)){
kyleliangus 12:5790e56a056f 45
kyleliangus 12:5790e56a056f 46 count0 = encoder0.getPulses() - initialCount0;
kyleliangus 12:5790e56a056f 47 count1 = encoder1.getPulses() - initialCount1;
kyleliangus 12:5790e56a056f 48
kyleliangus 12:5790e56a056f 49 error0 = count0 + desiredCount0; // moves forward
kyleliangus 12:5790e56a056f 50 error1 = count1 + desiredCount1; // moves backwards
kyleliangus 12:5790e56a056f 51
kyleliangus 12:5790e56a056f 52 speed0 = error0 * kp + speed0;
kyleliangus 12:5790e56a056f 53 speed1 = error1 * kp + speed1;
kyleliangus 12:5790e56a056f 54
kyleliangus 12:5790e56a056f 55 right_motor.move(speed0);
kyleliangus 12:5790e56a056f 56 left_motor.move(speed1);
kyleliangus 12:5790e56a056f 57
kyleliangus 12:5790e56a056f 58 serial.printf("ERROR=> 0:%f, 1:%f, SPEED=> 0:%f, 1:%f\n", error0, error1, speed0, speed1);
kyleliangus 12:5790e56a056f 59 serial.printf("Pulse Count=> e0:%d, e1:%d \r\n", encoder0.getPulses(),encoder1.getPulses());
kyleliangus 12:5790e56a056f 60 }
kyleliangus 12:5790e56a056f 61
kyleliangus 12:5790e56a056f 62 right_motor.brake();
kyleliangus 12:5790e56a056f 63 left_motor.brake();
kyleliangus 12:5790e56a056f 64 }
kyleliangus 12:5790e56a056f 65
kyleliangus 9:1d8e4da058cd 66 void moveForwardUntilWallIr() {
kyleliangus 9:1d8e4da058cd 67 double currentError = 0;
kyleliangus 9:1d8e4da058cd 68 double previousError = 0;
kyleliangus 9:1d8e4da058cd 69 double derivError = 0;
kyleliangus 9:1d8e4da058cd 70 double sumError = 0;
kyleliangus 9:1d8e4da058cd 71
vanshg 11:8fc2b703086b 72 double HIGH_PWM_VOLTAGE = 0.1;
kyleliangus 9:1d8e4da058cd 73
vanshg 11:8fc2b703086b 74 double rightSpeed = 0.1;
vanshg 11:8fc2b703086b 75 double leftSpeed = 0.1;
kyleliangus 9:1d8e4da058cd 76
kyleliangus 9:1d8e4da058cd 77 float ir2 = IRP_2.getSamples( SAMPLE_NUM );
kyleliangus 9:1d8e4da058cd 78 float ir3 = IRP_3.getSamples( SAMPLE_NUM );
kyleliangus 9:1d8e4da058cd 79
vanshg 11:8fc2b703086b 80 int count = encoder0.getPulses();
vanshg 11:8fc2b703086b 81 while ((IRP_1.getSamples( SAMPLE_NUM ) + IRP_4.getSamples( SAMPLE_NUM ) )/2 < 0.05f) {
vanshg 11:8fc2b703086b 82 int pulseCount = (encoder0.getPulses()-count) % 5600;
vanshg 11:8fc2b703086b 83 if (pulseCount > 5400 && pulseCount < 5800) {
vanshg 11:8fc2b703086b 84 blueLed.write(0);
vanshg 11:8fc2b703086b 85 } else {
vanshg 11:8fc2b703086b 86 blueLed.write(1);
vanshg 11:8fc2b703086b 87 }
vanshg 11:8fc2b703086b 88
vanshg 11:8fc2b703086b 89
kyleliangus 9:1d8e4da058cd 90 currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ;
kyleliangus 9:1d8e4da058cd 91 derivError = currentError - previousError;
kyleliangus 9:1d8e4da058cd 92 double PIDSum = IP_CONSTANT*currentError + II_CONSTANT*sumError + ID_CONSTANT*derivError;
kyleliangus 9:1d8e4da058cd 93 if (PIDSum > 0) { // this means the leftWheel is faster than the right. So right speeds up, left slows down
kyleliangus 9:1d8e4da058cd 94 rightSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE);
kyleliangus 9:1d8e4da058cd 95 leftSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE);
kyleliangus 9:1d8e4da058cd 96 } else { // r is faster than L. speed up l, slow down r
kyleliangus 9:1d8e4da058cd 97 rightSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE);
kyleliangus 9:1d8e4da058cd 98 leftSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE);
kyleliangus 9:1d8e4da058cd 99 }
kyleliangus 9:1d8e4da058cd 100
kyleliangus 9:1d8e4da058cd 101 if (leftSpeed > 0.30) leftSpeed = 0.30;
kyleliangus 9:1d8e4da058cd 102 if (leftSpeed < 0) leftSpeed = 0;
kyleliangus 9:1d8e4da058cd 103 if (rightSpeed > 0.30) rightSpeed = 0.30;
kyleliangus 9:1d8e4da058cd 104 if (rightSpeed < 0) rightSpeed = 0;
kyleliangus 9:1d8e4da058cd 105
kyleliangus 9:1d8e4da058cd 106 right_motor.forward(rightSpeed);
kyleliangus 9:1d8e4da058cd 107 left_motor.forward(leftSpeed);
kyleliangus 9:1d8e4da058cd 108
kyleliangus 9:1d8e4da058cd 109 previousError = currentError;
kyleliangus 9:1d8e4da058cd 110
kyleliangus 9:1d8e4da058cd 111 ir2 = IRP_2.getSamples( SAMPLE_NUM );
kyleliangus 9:1d8e4da058cd 112 ir3 = IRP_3.getSamples( SAMPLE_NUM );
kyleliangus 9:1d8e4da058cd 113
kyleliangus 9:1d8e4da058cd 114 }
kyleliangus 9:1d8e4da058cd 115 //sensor1Turn = IR_Sensor1.read();
kyleliangus 9:1d8e4da058cd 116 //sensor4Turn = IR_Sensor4.read();
kyleliangus 9:1d8e4da058cd 117
kyleliangus 9:1d8e4da058cd 118 //backward();
kyleliangus 9:1d8e4da058cd 119 wait_ms(40);
kyleliangus 9:1d8e4da058cd 120 //brake();
kyleliangus 9:1d8e4da058cd 121
kyleliangus 9:1d8e4da058cd 122 left_motor.brake();
kyleliangus 9:1d8e4da058cd 123 right_motor.brake();
kyleliangus 9:1d8e4da058cd 124 while( 1 )
kyleliangus 9:1d8e4da058cd 125 {
kyleliangus 9:1d8e4da058cd 126
kyleliangus 9:1d8e4da058cd 127 }
vanshg 10:810d1849da9d 128 }
vanshg 10:810d1849da9d 129
vanshg 11:8fc2b703086b 130 void printDipFlag() {
vanshg 11:8fc2b703086b 131 if (DEBUGGING) serial.printf("Flag value is %d", dipFlags);
vanshg 11:8fc2b703086b 132 }
vanshg 11:8fc2b703086b 133
vanshg 10:810d1849da9d 134 void enableButton1() {
vanshg 10:810d1849da9d 135 dipFlags |= BUTTON1_FLAG;
vanshg 11:8fc2b703086b 136 printDipFlag();
vanshg 10:810d1849da9d 137 }
vanshg 10:810d1849da9d 138 void enableButton2() {
vanshg 10:810d1849da9d 139 dipFlags |= BUTTON2_FLAG;
vanshg 11:8fc2b703086b 140 printDipFlag();
vanshg 10:810d1849da9d 141 }
vanshg 10:810d1849da9d 142 void enableButton3() {
vanshg 10:810d1849da9d 143 dipFlags |= BUTTON3_FLAG;
vanshg 11:8fc2b703086b 144 printDipFlag();
vanshg 10:810d1849da9d 145 }
vanshg 10:810d1849da9d 146 void enableButton4() {
vanshg 10:810d1849da9d 147 dipFlags |= BUTTON4_FLAG;
vanshg 11:8fc2b703086b 148 printDipFlag();
vanshg 10:810d1849da9d 149 }
vanshg 10:810d1849da9d 150 void disableButton1() {
vanshg 10:810d1849da9d 151 dipFlags &= ~BUTTON1_FLAG;
vanshg 11:8fc2b703086b 152 printDipFlag();
vanshg 10:810d1849da9d 153 }
vanshg 10:810d1849da9d 154 void disableButton2() {
vanshg 10:810d1849da9d 155 dipFlags &= ~BUTTON2_FLAG;
vanshg 11:8fc2b703086b 156 printDipFlag();
vanshg 10:810d1849da9d 157 }
vanshg 10:810d1849da9d 158 void disableButton3() {
vanshg 10:810d1849da9d 159 dipFlags &= ~BUTTON3_FLAG;
vanshg 11:8fc2b703086b 160 printDipFlag();
vanshg 10:810d1849da9d 161 }
vanshg 10:810d1849da9d 162 void disableButton4() {
vanshg 10:810d1849da9d 163 dipFlags &= ~BUTTON4_FLAG;
vanshg 11:8fc2b703086b 164 printDipFlag();
kyleliangus 9:1d8e4da058cd 165 }
christine222 3:880f15be8c72 166
sahilmgandhi 0:a03c771ab78e 167 int main()
sahilmgandhi 0:a03c771ab78e 168 {
kyleliangus 8:a0760acdc59e 169 //enableMotors();
christine222 3:880f15be8c72 170 //Set highest bandwidth.
sahilmgandhi 1:8a4b2f573923 171 gyro.setLpBandwidth(LPFBW_42HZ);
christine222 3:880f15be8c72 172 serial.baud(9600);
sahilmgandhi 7:6f5cb6377bd4 173 serial.printf("The gyro's address is %s", gyro.getWhoAmI());
sahilmgandhi 7:6f5cb6377bd4 174
sahilmgandhi 1:8a4b2f573923 175 wait (0.1);
sahilmgandhi 7:6f5cb6377bd4 176
christine222 3:880f15be8c72 177 // IR_1.write(1);
sahilmgandhi 2:771db996cee0 178 // IR_2.write(1);
sahilmgandhi 2:771db996cee0 179 // IR_3.write(1);
sahilmgandhi 2:771db996cee0 180 // IR_4.write(1);
christine222 3:880f15be8c72 181
sahilmgandhi 2:771db996cee0 182 redLed.write(1);
vanshg 11:8fc2b703086b 183 greenLed.write(1);
sahilmgandhi 2:771db996cee0 184 blueLed.write(1);
sahilmgandhi 1:8a4b2f573923 185
kyleliangus 9:1d8e4da058cd 186 //left_motor.forward(0.1);
kyleliangus 9:1d8e4da058cd 187 //right_motor.forward(0.1);
kyleliangus 8:a0760acdc59e 188
kyleliangus 8:a0760acdc59e 189 // PA_1 is A of right
kyleliangus 8:a0760acdc59e 190 // PA_0 is B of right
kyleliangus 8:a0760acdc59e 191 // PA_5 is A of left
kyleliangus 8:a0760acdc59e 192 // PB_3 is B of left
vanshg 11:8fc2b703086b 193 //QEI encoder0( PA_5, PB_3, NC, PULSES, QEI::X4_ENCODING );
vanshg 11:8fc2b703086b 194 // QEI encoder1( PA_1, PA_0, NC, PULSES, QEI::X4_ENCODING );
vanshg 10:810d1849da9d 195
vanshg 10:810d1849da9d 196 // TODO: Setting all the registers and what not for Quadrature Encoders
vanshg 10:810d1849da9d 197 RCC->APB1ENR |= 0x1001; // Enable clock for Tim2 (Bit 0) and Tim5 (Bit 3)
vanshg 10:810d1849da9d 198 RCC->AHB1ENR |= 0x11; // Enable GPIO port clock enables for Tim2(A) and Tim5(B)
vanshg 10:810d1849da9d 199 GPIOA->AFR[0] |= 0x10; // Set GPIO alternate function modes for Tim2
vanshg 10:810d1849da9d 200 GPIOB->AFR[0] |= 0x100; // Set GPIO alternate function modes for Tim5
vanshg 10:810d1849da9d 201
kyleliangus 12:5790e56a056f 202 // 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 203 // of alternate function specified
kyleliangus 12:5790e56a056f 204 // 4 modes sets AHB1ENR
kyleliangus 12:5790e56a056f 205 // Now TMR: enable clock with timer, APB1ENR
kyleliangus 12:5790e56a056f 206 // set period, autoreload value, ARR value 2^32-1, CR1 - TMR resets itself, ARPE and EN
kyleliangus 12:5790e56a056f 207 //
kyleliangus 12:5790e56a056f 208 // Encoder mode: disable timer before changing timer to encoder
kyleliangus 12:5790e56a056f 209 // CCMR1/2 1/2 depends on channel 1/2 or 3/4, depends on upper bits, depending which channels you use
kyleliangus 12:5790e56a056f 210 // CCMR sets sample rate and set the channel to input
kyleliangus 12:5790e56a056f 211 // CCER, which edge to trigger on, cannot be 11(not allowed for encoder mode), CCER for both channels
kyleliangus 12:5790e56a056f 212 // SMCR - encoder mode
kyleliangus 12:5790e56a056f 213 // CR1 reenabales
kyleliangus 12:5790e56a056f 214 // then read CNT reg of timer
kyleliangus 12:5790e56a056f 215
kyleliangus 12:5790e56a056f 216
vanshg 10:810d1849da9d 217 dipButton1.rise(&enableButton1);
vanshg 10:810d1849da9d 218 dipButton2.rise(&enableButton2);
vanshg 10:810d1849da9d 219 dipButton3.rise(&enableButton3);
vanshg 10:810d1849da9d 220 dipButton4.rise(&enableButton4);
vanshg 10:810d1849da9d 221
vanshg 10:810d1849da9d 222 dipButton1.fall(&disableButton1);
vanshg 10:810d1849da9d 223 dipButton2.fall(&disableButton2);
vanshg 10:810d1849da9d 224 dipButton3.fall(&disableButton3);
vanshg 10:810d1849da9d 225 dipButton4.fall(&disableButton4);
vanshg 10:810d1849da9d 226
sahilmgandhi 7:6f5cb6377bd4 227
christine222 3:880f15be8c72 228 while (1) {
vanshg 11:8fc2b703086b 229
kyleliangus 12:5790e56a056f 230 //moveForwardUntilWallIr();
kyleliangus 12:5790e56a056f 231 turnRight();
kyleliangus 12:5790e56a056f 232 wait(1);
kyleliangus 8:a0760acdc59e 233 //serial.printf("%i, %i, %i\n", gyro.getGyroX(), gyro.getGyroY(), gyro.getGyroZ());
vanshg 11:8fc2b703086b 234 // serial.printf("Pulse Count=> e0:%d, e1:%d \r\n", encoder0.getPulses(),encoder1.getPulses());
kyleliangus 9:1d8e4da058cd 235 //double currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ;
kyleliangus 9:1d8e4da058cd 236
kyleliangus 9:1d8e4da058cd 237 //serial.printf("IRS= >: %f, %f, %f, %f, %f \r\n",
kyleliangus 9:1d8e4da058cd 238 // IRP_1.getSamples( 100 ), IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ), IRP_4.getSamples(100), currentError );
kyleliangus 9:1d8e4da058cd 239
christine222 3:880f15be8c72 240 //reading = Rec_4.read();
christine222 3:880f15be8c72 241 // serial.printf("reading: %f\n", reading);
sahilmgandhi 1:8a4b2f573923 242 // redLed.write(0);
sahilmgandhi 1:8a4b2f573923 243 // wait_ms(1000);
sahilmgandhi 1:8a4b2f573923 244 // redLed.write(1);
sahilmgandhi 1:8a4b2f573923 245 // greenLed.write(0);
sahilmgandhi 1:8a4b2f573923 246 // wait_ms(1000);
sahilmgandhi 1:8a4b2f573923 247 // greenLed.write(1);
sahilmgandhi 1:8a4b2f573923 248 // blueLed.write(0);
sahilmgandhi 1:8a4b2f573923 249 // wait_ms(1000);
sahilmgandhi 1:8a4b2f573923 250 // blueLed.write(1);
christine222 3:880f15be8c72 251 }
sahilmgandhi 0:a03c771ab78e 252 }