Mouse code for the MacroRat
main.cpp@11:8fc2b703086b, 2017-05-07 (annotated)
- Committer:
- vanshg
- Date:
- Sun May 07 01:13:42 2017 +0000
- Revision:
- 11:8fc2b703086b
- Parent:
- 10:810d1849da9d
- Child:
- 12:5790e56a056f
Counting cells somewhat properly now
Who changed what in which revision?
User | Revision | Line number | New 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 | 9:1d8e4da058cd | 24 | void moveForwardUntilWallIr() { |
kyleliangus | 9:1d8e4da058cd | 25 | double currentError = 0; |
kyleliangus | 9:1d8e4da058cd | 26 | double previousError = 0; |
kyleliangus | 9:1d8e4da058cd | 27 | double derivError = 0; |
kyleliangus | 9:1d8e4da058cd | 28 | double sumError = 0; |
kyleliangus | 9:1d8e4da058cd | 29 | |
vanshg | 11:8fc2b703086b | 30 | double HIGH_PWM_VOLTAGE = 0.1; |
kyleliangus | 9:1d8e4da058cd | 31 | |
vanshg | 11:8fc2b703086b | 32 | double rightSpeed = 0.1; |
vanshg | 11:8fc2b703086b | 33 | double leftSpeed = 0.1; |
kyleliangus | 9:1d8e4da058cd | 34 | |
kyleliangus | 9:1d8e4da058cd | 35 | float ir2 = IRP_2.getSamples( SAMPLE_NUM ); |
kyleliangus | 9:1d8e4da058cd | 36 | float ir3 = IRP_3.getSamples( SAMPLE_NUM ); |
kyleliangus | 9:1d8e4da058cd | 37 | |
vanshg | 11:8fc2b703086b | 38 | int count = encoder0.getPulses(); |
vanshg | 11:8fc2b703086b | 39 | while ((IRP_1.getSamples( SAMPLE_NUM ) + IRP_4.getSamples( SAMPLE_NUM ) )/2 < 0.05f) { |
vanshg | 11:8fc2b703086b | 40 | int pulseCount = (encoder0.getPulses()-count) % 5600; |
vanshg | 11:8fc2b703086b | 41 | if (pulseCount > 5400 && pulseCount < 5800) { |
vanshg | 11:8fc2b703086b | 42 | blueLed.write(0); |
vanshg | 11:8fc2b703086b | 43 | } else { |
vanshg | 11:8fc2b703086b | 44 | blueLed.write(1); |
vanshg | 11:8fc2b703086b | 45 | } |
vanshg | 11:8fc2b703086b | 46 | |
vanshg | 11:8fc2b703086b | 47 | |
kyleliangus | 9:1d8e4da058cd | 48 | currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ; |
kyleliangus | 9:1d8e4da058cd | 49 | derivError = currentError - previousError; |
kyleliangus | 9:1d8e4da058cd | 50 | double PIDSum = IP_CONSTANT*currentError + II_CONSTANT*sumError + ID_CONSTANT*derivError; |
kyleliangus | 9:1d8e4da058cd | 51 | if (PIDSum > 0) { // this means the leftWheel is faster than the right. So right speeds up, left slows down |
kyleliangus | 9:1d8e4da058cd | 52 | rightSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE); |
kyleliangus | 9:1d8e4da058cd | 53 | leftSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE); |
kyleliangus | 9:1d8e4da058cd | 54 | } else { // r is faster than L. speed up l, slow down r |
kyleliangus | 9:1d8e4da058cd | 55 | rightSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE); |
kyleliangus | 9:1d8e4da058cd | 56 | leftSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE); |
kyleliangus | 9:1d8e4da058cd | 57 | } |
kyleliangus | 9:1d8e4da058cd | 58 | |
kyleliangus | 9:1d8e4da058cd | 59 | if (leftSpeed > 0.30) leftSpeed = 0.30; |
kyleliangus | 9:1d8e4da058cd | 60 | if (leftSpeed < 0) leftSpeed = 0; |
kyleliangus | 9:1d8e4da058cd | 61 | if (rightSpeed > 0.30) rightSpeed = 0.30; |
kyleliangus | 9:1d8e4da058cd | 62 | if (rightSpeed < 0) rightSpeed = 0; |
kyleliangus | 9:1d8e4da058cd | 63 | |
kyleliangus | 9:1d8e4da058cd | 64 | right_motor.forward(rightSpeed); |
kyleliangus | 9:1d8e4da058cd | 65 | left_motor.forward(leftSpeed); |
kyleliangus | 9:1d8e4da058cd | 66 | |
kyleliangus | 9:1d8e4da058cd | 67 | previousError = currentError; |
kyleliangus | 9:1d8e4da058cd | 68 | |
kyleliangus | 9:1d8e4da058cd | 69 | ir2 = IRP_2.getSamples( SAMPLE_NUM ); |
kyleliangus | 9:1d8e4da058cd | 70 | ir3 = IRP_3.getSamples( SAMPLE_NUM ); |
kyleliangus | 9:1d8e4da058cd | 71 | |
kyleliangus | 9:1d8e4da058cd | 72 | } |
kyleliangus | 9:1d8e4da058cd | 73 | //sensor1Turn = IR_Sensor1.read(); |
kyleliangus | 9:1d8e4da058cd | 74 | //sensor4Turn = IR_Sensor4.read(); |
kyleliangus | 9:1d8e4da058cd | 75 | |
kyleliangus | 9:1d8e4da058cd | 76 | //backward(); |
kyleliangus | 9:1d8e4da058cd | 77 | wait_ms(40); |
kyleliangus | 9:1d8e4da058cd | 78 | //brake(); |
kyleliangus | 9:1d8e4da058cd | 79 | |
kyleliangus | 9:1d8e4da058cd | 80 | left_motor.brake(); |
kyleliangus | 9:1d8e4da058cd | 81 | right_motor.brake(); |
kyleliangus | 9:1d8e4da058cd | 82 | while( 1 ) |
kyleliangus | 9:1d8e4da058cd | 83 | { |
kyleliangus | 9:1d8e4da058cd | 84 | |
kyleliangus | 9:1d8e4da058cd | 85 | } |
vanshg | 10:810d1849da9d | 86 | } |
vanshg | 10:810d1849da9d | 87 | |
vanshg | 11:8fc2b703086b | 88 | void printDipFlag() { |
vanshg | 11:8fc2b703086b | 89 | if (DEBUGGING) serial.printf("Flag value is %d", dipFlags); |
vanshg | 11:8fc2b703086b | 90 | } |
vanshg | 11:8fc2b703086b | 91 | |
vanshg | 10:810d1849da9d | 92 | void enableButton1() { |
vanshg | 10:810d1849da9d | 93 | dipFlags |= BUTTON1_FLAG; |
vanshg | 11:8fc2b703086b | 94 | printDipFlag(); |
vanshg | 10:810d1849da9d | 95 | } |
vanshg | 10:810d1849da9d | 96 | void enableButton2() { |
vanshg | 10:810d1849da9d | 97 | dipFlags |= BUTTON2_FLAG; |
vanshg | 11:8fc2b703086b | 98 | printDipFlag(); |
vanshg | 10:810d1849da9d | 99 | } |
vanshg | 10:810d1849da9d | 100 | void enableButton3() { |
vanshg | 10:810d1849da9d | 101 | dipFlags |= BUTTON3_FLAG; |
vanshg | 11:8fc2b703086b | 102 | printDipFlag(); |
vanshg | 10:810d1849da9d | 103 | } |
vanshg | 10:810d1849da9d | 104 | void enableButton4() { |
vanshg | 10:810d1849da9d | 105 | dipFlags |= BUTTON4_FLAG; |
vanshg | 11:8fc2b703086b | 106 | printDipFlag(); |
vanshg | 10:810d1849da9d | 107 | } |
vanshg | 10:810d1849da9d | 108 | void disableButton1() { |
vanshg | 10:810d1849da9d | 109 | dipFlags &= ~BUTTON1_FLAG; |
vanshg | 11:8fc2b703086b | 110 | printDipFlag(); |
vanshg | 10:810d1849da9d | 111 | } |
vanshg | 10:810d1849da9d | 112 | void disableButton2() { |
vanshg | 10:810d1849da9d | 113 | dipFlags &= ~BUTTON2_FLAG; |
vanshg | 11:8fc2b703086b | 114 | printDipFlag(); |
vanshg | 10:810d1849da9d | 115 | } |
vanshg | 10:810d1849da9d | 116 | void disableButton3() { |
vanshg | 10:810d1849da9d | 117 | dipFlags &= ~BUTTON3_FLAG; |
vanshg | 11:8fc2b703086b | 118 | printDipFlag(); |
vanshg | 10:810d1849da9d | 119 | } |
vanshg | 10:810d1849da9d | 120 | void disableButton4() { |
vanshg | 10:810d1849da9d | 121 | dipFlags &= ~BUTTON4_FLAG; |
vanshg | 11:8fc2b703086b | 122 | printDipFlag(); |
kyleliangus | 9:1d8e4da058cd | 123 | } |
christine222 | 3:880f15be8c72 | 124 | |
sahilmgandhi | 0:a03c771ab78e | 125 | int main() |
sahilmgandhi | 0:a03c771ab78e | 126 | { |
kyleliangus | 8:a0760acdc59e | 127 | //enableMotors(); |
christine222 | 3:880f15be8c72 | 128 | //Set highest bandwidth. |
sahilmgandhi | 1:8a4b2f573923 | 129 | gyro.setLpBandwidth(LPFBW_42HZ); |
christine222 | 3:880f15be8c72 | 130 | serial.baud(9600); |
sahilmgandhi | 7:6f5cb6377bd4 | 131 | serial.printf("The gyro's address is %s", gyro.getWhoAmI()); |
sahilmgandhi | 7:6f5cb6377bd4 | 132 | |
sahilmgandhi | 1:8a4b2f573923 | 133 | wait (0.1); |
sahilmgandhi | 7:6f5cb6377bd4 | 134 | |
christine222 | 3:880f15be8c72 | 135 | // IR_1.write(1); |
sahilmgandhi | 2:771db996cee0 | 136 | // IR_2.write(1); |
sahilmgandhi | 2:771db996cee0 | 137 | // IR_3.write(1); |
sahilmgandhi | 2:771db996cee0 | 138 | // IR_4.write(1); |
christine222 | 3:880f15be8c72 | 139 | |
sahilmgandhi | 2:771db996cee0 | 140 | redLed.write(1); |
vanshg | 11:8fc2b703086b | 141 | greenLed.write(1); |
sahilmgandhi | 2:771db996cee0 | 142 | blueLed.write(1); |
sahilmgandhi | 1:8a4b2f573923 | 143 | |
kyleliangus | 9:1d8e4da058cd | 144 | //left_motor.forward(0.1); |
kyleliangus | 9:1d8e4da058cd | 145 | //right_motor.forward(0.1); |
kyleliangus | 8:a0760acdc59e | 146 | |
kyleliangus | 8:a0760acdc59e | 147 | // PA_1 is A of right |
kyleliangus | 8:a0760acdc59e | 148 | // PA_0 is B of right |
kyleliangus | 8:a0760acdc59e | 149 | // PA_5 is A of left |
kyleliangus | 8:a0760acdc59e | 150 | // PB_3 is B of left |
vanshg | 11:8fc2b703086b | 151 | //QEI encoder0( PA_5, PB_3, NC, PULSES, QEI::X4_ENCODING ); |
vanshg | 11:8fc2b703086b | 152 | // QEI encoder1( PA_1, PA_0, NC, PULSES, QEI::X4_ENCODING ); |
vanshg | 10:810d1849da9d | 153 | |
vanshg | 10:810d1849da9d | 154 | // TODO: Setting all the registers and what not for Quadrature Encoders |
vanshg | 10:810d1849da9d | 155 | RCC->APB1ENR |= 0x1001; // Enable clock for Tim2 (Bit 0) and Tim5 (Bit 3) |
vanshg | 10:810d1849da9d | 156 | RCC->AHB1ENR |= 0x11; // Enable GPIO port clock enables for Tim2(A) and Tim5(B) |
vanshg | 10:810d1849da9d | 157 | GPIOA->AFR[0] |= 0x10; // Set GPIO alternate function modes for Tim2 |
vanshg | 10:810d1849da9d | 158 | GPIOB->AFR[0] |= 0x100; // Set GPIO alternate function modes for Tim5 |
vanshg | 10:810d1849da9d | 159 | |
vanshg | 10:810d1849da9d | 160 | dipButton1.rise(&enableButton1); |
vanshg | 10:810d1849da9d | 161 | dipButton2.rise(&enableButton2); |
vanshg | 10:810d1849da9d | 162 | dipButton3.rise(&enableButton3); |
vanshg | 10:810d1849da9d | 163 | dipButton4.rise(&enableButton4); |
vanshg | 10:810d1849da9d | 164 | |
vanshg | 10:810d1849da9d | 165 | dipButton1.fall(&disableButton1); |
vanshg | 10:810d1849da9d | 166 | dipButton2.fall(&disableButton2); |
vanshg | 10:810d1849da9d | 167 | dipButton3.fall(&disableButton3); |
vanshg | 10:810d1849da9d | 168 | dipButton4.fall(&disableButton4); |
vanshg | 10:810d1849da9d | 169 | |
sahilmgandhi | 7:6f5cb6377bd4 | 170 | |
christine222 | 3:880f15be8c72 | 171 | while (1) { |
vanshg | 11:8fc2b703086b | 172 | |
kyleliangus | 9:1d8e4da058cd | 173 | moveForwardUntilWallIr(); |
kyleliangus | 8:a0760acdc59e | 174 | wait(0.1); |
kyleliangus | 8:a0760acdc59e | 175 | //serial.printf("%i, %i, %i\n", gyro.getGyroX(), gyro.getGyroY(), gyro.getGyroZ()); |
vanshg | 11:8fc2b703086b | 176 | // serial.printf("Pulse Count=> e0:%d, e1:%d \r\n", encoder0.getPulses(),encoder1.getPulses()); |
kyleliangus | 9:1d8e4da058cd | 177 | //double currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ; |
kyleliangus | 9:1d8e4da058cd | 178 | |
kyleliangus | 9:1d8e4da058cd | 179 | //serial.printf("IRS= >: %f, %f, %f, %f, %f \r\n", |
kyleliangus | 9:1d8e4da058cd | 180 | // IRP_1.getSamples( 100 ), IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ), IRP_4.getSamples(100), currentError ); |
kyleliangus | 9:1d8e4da058cd | 181 | |
christine222 | 3:880f15be8c72 | 182 | //reading = Rec_4.read(); |
christine222 | 3:880f15be8c72 | 183 | // serial.printf("reading: %f\n", reading); |
sahilmgandhi | 1:8a4b2f573923 | 184 | // redLed.write(0); |
sahilmgandhi | 1:8a4b2f573923 | 185 | // wait_ms(1000); |
sahilmgandhi | 1:8a4b2f573923 | 186 | // redLed.write(1); |
sahilmgandhi | 1:8a4b2f573923 | 187 | // greenLed.write(0); |
sahilmgandhi | 1:8a4b2f573923 | 188 | // wait_ms(1000); |
sahilmgandhi | 1:8a4b2f573923 | 189 | // greenLed.write(1); |
sahilmgandhi | 1:8a4b2f573923 | 190 | // blueLed.write(0); |
sahilmgandhi | 1:8a4b2f573923 | 191 | // wait_ms(1000); |
sahilmgandhi | 1:8a4b2f573923 | 192 | // blueLed.write(1); |
christine222 | 3:880f15be8c72 | 193 | } |
sahilmgandhi | 0:a03c771ab78e | 194 | } |