hello
Dependencies: AVEncoder QEI mbed-src-AV
main.cpp@2:5f9b78950a17, 2015-11-10 (annotated)
- Committer:
- intgsull
- Date:
- Tue Nov 10 07:29:32 2015 +0000
- Revision:
- 2:5f9b78950a17
- Parent:
- 1:5b9fa1823663
- Child:
- 4:453d534b1c1d
Lab 2 Complete!
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
intgsull | 0:fa523db3f4f5 | 1 | #include "mbed.h" |
intgsull | 1:5b9fa1823663 | 2 | #include "QEI.h" |
intgsull | 0:fa523db3f4f5 | 3 | |
intgsull | 2:5f9b78950a17 | 4 | PwmOut motor1_reverse(PA_6); |
intgsull | 1:5b9fa1823663 | 5 | PwmOut motor1_forward(PB_10); |
intgsull | 0:fa523db3f4f5 | 6 | PwmOut motor2_forward(PA_7); |
intgsull | 0:fa523db3f4f5 | 7 | PwmOut motor2_reverse(PB_6); |
intgsull | 0:fa523db3f4f5 | 8 | |
intgsull | 2:5f9b78950a17 | 9 | //DigitalIn encoder1A(PA_15); |
intgsull | 2:5f9b78950a17 | 10 | //DigitalIn encoder1B(PB_3); |
intgsull | 2:5f9b78950a17 | 11 | //DigitalIn encoder2A(PA_1); |
intgsull | 2:5f9b78950a17 | 12 | //DigitalIn encoder2B(PA_10); |
intgsull | 2:5f9b78950a17 | 13 | |
intgsull | 2:5f9b78950a17 | 14 | |
intgsull | 2:5f9b78950a17 | 15 | |
intgsull | 2:5f9b78950a17 | 16 | //Gyro |
intgsull | 2:5f9b78950a17 | 17 | AnalogIn _gyro(PA_0); |
intgsull | 2:5f9b78950a17 | 18 | AnalogIn gyro_cal(PC_1); //no |
intgsull | 1:5b9fa1823663 | 19 | |
intgsull | 2:5f9b78950a17 | 20 | //Left Front IR |
intgsull | 2:5f9b78950a17 | 21 | DigitalOut eLF(PC_3); |
intgsull | 2:5f9b78950a17 | 22 | AnalogIn rLF(PC_0); |
intgsull | 2:5f9b78950a17 | 23 | //PC_4 is an ADC |
intgsull | 2:5f9b78950a17 | 24 | |
intgsull | 2:5f9b78950a17 | 25 | //Left Side IR |
intgsull | 2:5f9b78950a17 | 26 | DigitalOut eLS(PC_2); |
intgsull | 2:5f9b78950a17 | 27 | AnalogIn rLS(PC_1); |
intgsull | 2:5f9b78950a17 | 28 | |
intgsull | 2:5f9b78950a17 | 29 | //Right Front IR |
intgsull | 2:5f9b78950a17 | 30 | DigitalOut eRF(PC_12); |
intgsull | 2:5f9b78950a17 | 31 | AnalogIn rRF(PA_4); |
intgsull | 2:5f9b78950a17 | 32 | |
intgsull | 2:5f9b78950a17 | 33 | //Right Side IR |
intgsull | 2:5f9b78950a17 | 34 | DigitalOut eRS(PC_15); |
intgsull | 2:5f9b78950a17 | 35 | AnalogIn rRS(PB_0); |
intgsull | 1:5b9fa1823663 | 36 | |
intgsull | 0:fa523db3f4f5 | 37 | DigitalOut myled(LED1); |
intgsull | 0:fa523db3f4f5 | 38 | |
intgsull | 2:5f9b78950a17 | 39 | QEI LENC (PA_15, PB_3, NC, 100 ); |
intgsull | 1:5b9fa1823663 | 40 | QEI RENC (PA_1, PA_10, NC, 100 ); |
intgsull | 1:5b9fa1823663 | 41 | |
intgsull | 1:5b9fa1823663 | 42 | int distance = 0; |
intgsull | 1:5b9fa1823663 | 43 | |
intgsull | 0:fa523db3f4f5 | 44 | void rightForward(float speed); |
intgsull | 0:fa523db3f4f5 | 45 | void leftForward(float speed); |
intgsull | 0:fa523db3f4f5 | 46 | |
intgsull | 2:5f9b78950a17 | 47 | float read_gyro( void ); |
intgsull | 2:5f9b78950a17 | 48 | |
intgsull | 2:5f9b78950a17 | 49 | Serial pc (SERIAL_TX, SERIAL_RX); |
intgsull | 1:5b9fa1823663 | 50 | |
intgsull | 0:fa523db3f4f5 | 51 | int main() { |
intgsull | 2:5f9b78950a17 | 52 | // LENC.reset(); |
intgsull | 2:5f9b78950a17 | 53 | // RENC.reset(); |
intgsull | 2:5f9b78950a17 | 54 | eRS = 0; |
intgsull | 2:5f9b78950a17 | 55 | eRF = 0; |
intgsull | 2:5f9b78950a17 | 56 | eLS = 0; |
intgsull | 2:5f9b78950a17 | 57 | eLF = 0; |
intgsull | 2:5f9b78950a17 | 58 | |
intgsull | 2:5f9b78950a17 | 59 | myled = 1; |
intgsull | 1:5b9fa1823663 | 60 | while(1) { |
intgsull | 2:5f9b78950a17 | 61 | |
intgsull | 1:5b9fa1823663 | 62 | motor1_reverse = 0; |
intgsull | 1:5b9fa1823663 | 63 | motor1_forward = 0; |
intgsull | 0:fa523db3f4f5 | 64 | |
intgsull | 1:5b9fa1823663 | 65 | motor2_forward = 0; |
intgsull | 1:5b9fa1823663 | 66 | motor2_reverse = 0; |
intgsull | 1:5b9fa1823663 | 67 | |
intgsull | 2:5f9b78950a17 | 68 | eRS = 0; |
intgsull | 2:5f9b78950a17 | 69 | eRF = 0; |
intgsull | 2:5f9b78950a17 | 70 | eLS = 0; |
intgsull | 2:5f9b78950a17 | 71 | eLF = 0; |
intgsull | 2:5f9b78950a17 | 72 | |
intgsull | 2:5f9b78950a17 | 73 | pc.printf("LEDS off\r\n"); |
intgsull | 2:5f9b78950a17 | 74 | wait(1); |
intgsull | 2:5f9b78950a17 | 75 | pc.printf("Left Front: \t%f\r\n", rLF.read()); |
intgsull | 2:5f9b78950a17 | 76 | pc.printf("Left Side: \t%f\r\n", rLS.read()); |
intgsull | 2:5f9b78950a17 | 77 | pc.printf("Right Front: \t%f\r\n", rRS.read()); |
intgsull | 2:5f9b78950a17 | 78 | pc.printf("Right Side: \t%f\r\n\n", rRF.read()); |
intgsull | 2:5f9b78950a17 | 79 | wait(.5); |
intgsull | 2:5f9b78950a17 | 80 | |
intgsull | 2:5f9b78950a17 | 81 | eRS = 1; |
intgsull | 2:5f9b78950a17 | 82 | eRF = 1; |
intgsull | 2:5f9b78950a17 | 83 | eLS = 1; |
intgsull | 2:5f9b78950a17 | 84 | eLF = 1; |
intgsull | 2:5f9b78950a17 | 85 | |
intgsull | 2:5f9b78950a17 | 86 | pc.printf("LEDS on\r\n"); |
intgsull | 2:5f9b78950a17 | 87 | wait(1); |
intgsull | 2:5f9b78950a17 | 88 | pc.printf("Left Front: \t%f\r\n", rLF.read()); |
intgsull | 2:5f9b78950a17 | 89 | pc.printf("Left Side: \t%f\r\n", rLS.read()); |
intgsull | 2:5f9b78950a17 | 90 | pc.printf("Right Front: \t%f\r\n", rRS.read()); |
intgsull | 2:5f9b78950a17 | 91 | pc.printf("Right Side: \t%f\r\n\n", rRF.read()); |
intgsull | 2:5f9b78950a17 | 92 | // pc.printf("%f\r\n", _gyro.read()); |
intgsull | 2:5f9b78950a17 | 93 | // wait(0.5); |
intgsull | 2:5f9b78950a17 | 94 | } |
intgsull | 1:5b9fa1823663 | 95 | |
intgsull | 2:5f9b78950a17 | 96 | |
intgsull | 2:5f9b78950a17 | 97 | // int in = encoder1A.read(); |
intgsull | 2:5f9b78950a17 | 98 | // if(in==1) |
intgsull | 2:5f9b78950a17 | 99 | // myled=1; |
intgsull | 2:5f9b78950a17 | 100 | // else |
intgsull | 2:5f9b78950a17 | 101 | // myled=0; |
intgsull | 2:5f9b78950a17 | 102 | |
intgsull | 2:5f9b78950a17 | 103 | //} |
intgsull | 1:5b9fa1823663 | 104 | // |
intgsull | 1:5b9fa1823663 | 105 | // motor1_reverse = 1.0; |
intgsull | 1:5b9fa1823663 | 106 | // motor1_forward = 1.0; |
intgsull | 1:5b9fa1823663 | 107 | // |
intgsull | 1:5b9fa1823663 | 108 | // motor2_forward = 1.0; |
intgsull | 1:5b9fa1823663 | 109 | // motor2_reverse = 1.0; |
intgsull | 1:5b9fa1823663 | 110 | //rightForward(0.5); |
intgsull | 1:5b9fa1823663 | 111 | //leftForward(-0.5); |
intgsull | 0:fa523db3f4f5 | 112 | } |
intgsull | 2:5f9b78950a17 | 113 | |
intgsull | 2:5f9b78950a17 | 114 | |
intgsull | 2:5f9b78950a17 | 115 | float read_gyro ( void ){ |
intgsull | 2:5f9b78950a17 | 116 | // Gyro is a ST Micro LY3200ALH Yaw-rate Gyro: 0.67mV / d.p.s, 1.5V offset |
intgsull | 2:5f9b78950a17 | 117 | float r_gyro = _gyro.read(); // Get the Analog value from the STM32 |
intgsull | 2:5f9b78950a17 | 118 | r_gyro *= gyro_cal; // NB 3.4 seemed to work better that 3.3 but better to run calibration routine |
intgsull | 2:5f9b78950a17 | 119 | r_gyro = 1.5 - r_gyro; // 1.5V off-set, invert sign |
intgsull | 2:5f9b78950a17 | 120 | r_gyro *= 1492.5; // Convert to DPS (+ve = clockwise, -ve = c.clockwise) |
intgsull | 2:5f9b78950a17 | 121 | return r_gyro; |
intgsull | 2:5f9b78950a17 | 122 | } |
intgsull | 2:5f9b78950a17 | 123 | |
intgsull | 1:5b9fa1823663 | 124 | void moveForward(float speed) |
intgsull | 1:5b9fa1823663 | 125 | { |
intgsull | 1:5b9fa1823663 | 126 | motor1_forward = speed; |
intgsull | 1:5b9fa1823663 | 127 | motor2_forward = speed; |
intgsull | 1:5b9fa1823663 | 128 | |
intgsull | 1:5b9fa1823663 | 129 | motor1_reverse = 0; |
intgsull | 1:5b9fa1823663 | 130 | motor2_reverse = 0; |
intgsull | 1:5b9fa1823663 | 131 | } |
intgsull | 1:5b9fa1823663 | 132 | |
intgsull | 1:5b9fa1823663 | 133 | |
intgsull | 1:5b9fa1823663 | 134 | |
intgsull | 0:fa523db3f4f5 | 135 | void rightForward(float speed) { |
intgsull | 0:fa523db3f4f5 | 136 | if (speed == 0) { |
intgsull | 0:fa523db3f4f5 | 137 | motor1_forward = 1.0; |
intgsull | 0:fa523db3f4f5 | 138 | motor1_reverse = 1.0; |
intgsull | 0:fa523db3f4f5 | 139 | } |
intgsull | 0:fa523db3f4f5 | 140 | |
intgsull | 0:fa523db3f4f5 | 141 | if (speed > 0) { |
intgsull | 0:fa523db3f4f5 | 142 | motor1_forward = speed; |
intgsull | 0:fa523db3f4f5 | 143 | motor1_reverse = 0; |
intgsull | 0:fa523db3f4f5 | 144 | } |
intgsull | 0:fa523db3f4f5 | 145 | |
intgsull | 0:fa523db3f4f5 | 146 | else { |
intgsull | 0:fa523db3f4f5 | 147 | motor1_forward = 0; |
intgsull | 0:fa523db3f4f5 | 148 | motor1_reverse = -speed; |
intgsull | 0:fa523db3f4f5 | 149 | } |
intgsull | 0:fa523db3f4f5 | 150 | } |
intgsull | 0:fa523db3f4f5 | 151 | |
intgsull | 0:fa523db3f4f5 | 152 | void leftForward(float speed) { |
intgsull | 0:fa523db3f4f5 | 153 | if (speed == 0) { |
intgsull | 0:fa523db3f4f5 | 154 | motor2_forward = 1.0; |
intgsull | 0:fa523db3f4f5 | 155 | motor2_reverse = 1.0; |
intgsull | 0:fa523db3f4f5 | 156 | } |
intgsull | 0:fa523db3f4f5 | 157 | |
intgsull | 0:fa523db3f4f5 | 158 | if (speed > 0) { |
intgsull | 0:fa523db3f4f5 | 159 | motor2_forward = speed; |
intgsull | 0:fa523db3f4f5 | 160 | motor2_reverse = 0; |
intgsull | 0:fa523db3f4f5 | 161 | } |
intgsull | 0:fa523db3f4f5 | 162 | |
intgsull | 0:fa523db3f4f5 | 163 | else { |
intgsull | 0:fa523db3f4f5 | 164 | motor2_forward = 0; |
intgsull | 0:fa523db3f4f5 | 165 | motor2_reverse = -speed; |
intgsull | 0:fa523db3f4f5 | 166 | } |
intgsull | 0:fa523db3f4f5 | 167 | } |