hello
Dependencies: AVEncoder QEI mbed-src-AV
Diff: main.cpp
- Revision:
- 2:5f9b78950a17
- Parent:
- 1:5b9fa1823663
- Child:
- 4:453d534b1c1d
--- a/main.cpp Tue Oct 20 03:07:42 2015 +0000 +++ b/main.cpp Tue Nov 10 07:29:32 2015 +0000 @@ -1,21 +1,42 @@ #include "mbed.h" #include "QEI.h" -PwmOut motor1_reverse(PB_4); +PwmOut motor1_reverse(PA_6); PwmOut motor1_forward(PB_10); PwmOut motor2_forward(PA_7); PwmOut motor2_reverse(PB_6); -// -DigitalIn encoder1A(PC_15); -DigitalIn encoder1B(PB_3); -DigitalIn enconder2A(PA_1); -DigitalIn enconder2B(PA_2); // we need to check things now. +//DigitalIn encoder1A(PA_15); +//DigitalIn encoder1B(PB_3); +//DigitalIn encoder2A(PA_1); +//DigitalIn encoder2B(PA_10); + + + +//Gyro +AnalogIn _gyro(PA_0); +AnalogIn gyro_cal(PC_1); //no +//Left Front IR +DigitalOut eLF(PC_3); +AnalogIn rLF(PC_0); + //PC_4 is an ADC + +//Left Side IR +DigitalOut eLS(PC_2); +AnalogIn rLS(PC_1); + +//Right Front IR +DigitalOut eRF(PC_12); +AnalogIn rRF(PA_4); + +//Right Side IR +DigitalOut eRS(PC_15); +AnalogIn rRS(PB_0); DigitalOut myled(LED1); -QEI LENC (PC_15, PB_3, NC, 100 ); +QEI LENC (PA_15, PB_3, NC, 100 ); QEI RENC (PA_1, PA_10, NC, 100 ); int distance = 0; @@ -23,42 +44,63 @@ void rightForward(float speed); void leftForward(float speed); -Serial pc (USBTX, USBRX); +float read_gyro( void ); + +Serial pc (SERIAL_TX, SERIAL_RX); int main() { - LENC.reset(); - RENC.reset(); - - pc. printf ( "Hello World!!!!!!\r\n" ); - - myled = 0; + // LENC.reset(); + // RENC.reset(); + eRS = 0; + eRF = 0; + eLS = 0; + eLF = 0; + + myled = 1; while(1) { -// motor1_reverse = 0.5; -// motor1_forward = 0; -// -// motor2_forward = 0.5; -// motor2_reverse = 0; - + motor1_reverse = 0; motor1_forward = 0; motor2_forward = 0; motor2_reverse = 0; - int in = encoder1A.read(); + eRS = 0; + eRF = 0; + eLS = 0; + eLF = 0; + + pc.printf("LEDS off\r\n"); + wait(1); + pc.printf("Left Front: \t%f\r\n", rLF.read()); + pc.printf("Left Side: \t%f\r\n", rLS.read()); + pc.printf("Right Front: \t%f\r\n", rRS.read()); + pc.printf("Right Side: \t%f\r\n\n", rRF.read()); + wait(.5); + + eRS = 1; + eRF = 1; + eLS = 1; + eLF = 1; + + pc.printf("LEDS on\r\n"); + wait(1); + pc.printf("Left Front: \t%f\r\n", rLF.read()); + pc.printf("Left Side: \t%f\r\n", rLS.read()); + pc.printf("Right Front: \t%f\r\n", rRS.read()); + pc.printf("Right Side: \t%f\r\n\n", rRF.read()); + // pc.printf("%f\r\n", _gyro.read()); +// wait(0.5); + } - while ( in == 1 ) - { - myled = 1; - wait(4); - in = encoder1A.read(); - } - while ( in == 0 ) - { - myled = 0; - in = encoder1A.read(); - } - } + + // int in = encoder1A.read(); + // if(in==1) + // myled=1; + // else + // myled=0; + + //} // // motor1_reverse = 1.0; // motor1_forward = 1.0; @@ -68,7 +110,17 @@ //rightForward(0.5); //leftForward(-0.5); } - + + + float read_gyro ( void ){ + // Gyro is a ST Micro LY3200ALH Yaw-rate Gyro: 0.67mV / d.p.s, 1.5V offset + float r_gyro = _gyro.read(); // Get the Analog value from the STM32 + r_gyro *= gyro_cal; // NB 3.4 seemed to work better that 3.3 but better to run calibration routine + r_gyro = 1.5 - r_gyro; // 1.5V off-set, invert sign + r_gyro *= 1492.5; // Convert to DPS (+ve = clockwise, -ve = c.clockwise) + return r_gyro; + } + void moveForward(float speed) { motor1_forward = speed;