zhouhang shao / Mbed 2 deprecated test

test

Dependencies:   RemoteIR mbed

main.cpp@5:5d34a8feffe2, 2017-05-09 (annotated)

Committer:
szh66
Date:
Tue May 09 01:21:47 2017 +0000
Revision:
5:5d34a8feffe2
Parent:
4:86927e61e1e3
Child:
6:9f698d1b2996
add gyro test case

Who changed what in which revision?

UserRevisionLine numberNew contents of line
szh66 0:3c59bc5c9388 1 /**
szh66 0:3c59bc5c9388 2 * This is simple program to test usart with IR sensors.
szh66 0:3c59bc5c9388 3 */
szh66 0:3c59bc5c9388 4 #include "mbed.h"
szh66 1:11970e541ecf 5 #include "IRSensor.h"
szh66 3:34a763e93423 6 #include "encoder.h"
szh66 3:34a763e93423 7 //#include "motor.h"
szh66 2:b78dfa2afe92 8 Serial serial (PA_9, PA_10);
FengjunyanLi 4:86927e61e1e3 9 IRSensor testIR_RIGHT(PB_0, PA_4); //emmiter and receiver right most sensor
FengjunyanLi 4:86927e61e1e3 10 IRSensor testIR_LEFT(PA_8, PC_5); //emmiter and receiver left most sensor
FengjunyanLi 4:86927e61e1e3 11 IRSensor testIR_TOP1(PC_6, PA_7); //emmiter and receiver TOP LEFT sensor
FengjunyanLi 4:86927e61e1e3 12 IRSensor testIR_TOP2(PB_10, PA_6); //emmiter and receiver TOP RIGHT sensor
FengjunyanLi 4:86927e61e1e3 13 IRSensor testIR_DIAG_LEFT(PC_7, PC_4); //emmiter and receiver DIAG LEFT sensor
FengjunyanLi 4:86927e61e1e3 14 IRSensor testIR_DIAG_RIGHT(PB_1, PA_5); //emmiter and receiver DIAG RIGHTsensor
szh66 2:b78dfa2afe92 15 DigitalOut testLed(PB_12);
szh66 2:b78dfa2afe92 16 DigitalOut testLed2(PB_13);
szh66 2:b78dfa2afe92 17 DigitalOut testLed3(PB_14);
szh66 2:b78dfa2afe92 18 DigitalOut testLed4(PB_15);
szh66 2:b78dfa2afe92 19 AnalogIn Ain(PA_3);
szh66 5:5d34a8feffe2 20 AnalogIn gyro(PC_1);
szh66 5:5d34a8feffe2 21 DigitalOut gyro_sleep(PC_2);
szh66 5:5d34a8feffe2 22
szh66 2:b78dfa2afe92 23 //Motor leftMotor(PB_7, PB_6);
FengjunyanLi 4:86927e61e1e3 24 //Motor rightMotor(PB_9, PB_8);
szh66 3:34a763e93423 25 // pwm
szh66 3:34a763e93423 26 PwmOut motor_pwm_left_sig(PB_7);
szh66 3:34a763e93423 27 DigitalOut dir_left(PB_6);
szh66 3:34a763e93423 28 //// dir
szh66 3:34a763e93423 29 PwmOut motor_pwm_right_sig(PB_9);
szh66 3:34a763e93423 30 DigitalOut dir_right(PB_8);
szh66 3:34a763e93423 31
szh66 3:34a763e93423 32
FengjunyanLi 4:86927e61e1e3 33
szh66 3:34a763e93423 34 Encoder leftEncoder(PC_9, PC_8);
szh66 0:3c59bc5c9388 35
szh66 0:3c59bc5c9388 36 int main ()
szh66 0:3c59bc5c9388 37 {
szh66 1:11970e541ecf 38 serial.printf ("Start Program\n");
szh66 2:b78dfa2afe92 39 //leftMotor = 0.1f;
szh66 1:11970e541ecf 40
FengjunyanLi 4:86927e61e1e3 41
FengjunyanLi 4:86927e61e1e3 42 // motor_pwm_left_sig.period(0.001f);
FengjunyanLi 4:86927e61e1e3 43 // motor_pwm_left_sig.write(0.2f);
FengjunyanLi 4:86927e61e1e3 44 // motor_pwm_right_sig.period(0.001f);
FengjunyanLi 4:86927e61e1e3 45 // motor_pwm_right_sig.write(0.6f);
FengjunyanLi 4:86927e61e1e3 46 // dir_left = 0;
FengjunyanLi 4:86927e61e1e3 47 // dir_right = 1;
FengjunyanLi 4:86927e61e1e3 48 //
FengjunyanLi 4:86927e61e1e3 49 // wait(7);
FengjunyanLi 4:86927e61e1e3 50 // motor_pwm_right_sig = 0;
FengjunyanLi 4:86927e61e1e3 51 // dir_right = 0;
FengjunyanLi 4:86927e61e1e3 52 // motor_pwm_left_sig = 0;
FengjunyanLi 4:86927e61e1e3 53
FengjunyanLi 4:86927e61e1e3 54
szh66 5:5d34a8feffe2 55 //double angle
FengjunyanLi 4:86927e61e1e3 56
szh66 0:3c59bc5c9388 57 while (1)
szh66 0:3c59bc5c9388 58 {
szh66 2:b78dfa2afe92 59 testLed2 = 1;
FengjunyanLi 4:86927e61e1e3 60 testLed3 = 1;
szh66 2:b78dfa2afe92 61 wait (0.2);
FengjunyanLi 4:86927e61e1e3 62 testLed2 = 0;
FengjunyanLi 4:86927e61e1e3 63 testLed3 = 0;
szh66 2:b78dfa2afe92 64 wait (0.2);
FengjunyanLi 4:86927e61e1e3 65
FengjunyanLi 4:86927e61e1e3 66 // serial.printf("Left sensor: %f\r\n", testIR_LEFT.readIR());
FengjunyanLi 4:86927e61e1e3 67 // serial.printf("Left Diagnal %f\r\n", testIR_DIAG_LEFT.readIR());
FengjunyanLi 4:86927e61e1e3 68 serial.printf("TOP LEFT %f\r\n", testIR_TOP1.readIR());
FengjunyanLi 4:86927e61e1e3 69 serial.printf("TOP RIGHT %f\r\n", testIR_TOP2.readIR());
FengjunyanLi 4:86927e61e1e3 70 // serial.printf("Right Diagnal %f\r\n", testIR_DIAG_RIGHT.readIR());
FengjunyanLi 4:86927e61e1e3 71 // serial.printf("Right sensor: %f\r\n", testIR_RIGHT.readIR());
FengjunyanLi 4:86927e61e1e3 72 wait(1);
szh66 5:5d34a8feffe2 73 //______________TEST GYRO
szh66 5:5d34a8feffe2 74 double offset = .4451;
szh66 5:5d34a8feffe2 75 gyro_sleep = 1;
szh66 5:5d34a8feffe2 76 double gyro_value = (gyro.read() - offset) * .67 / offset * 600; // Offset of .4455 measured experimentally
szh66 5:5d34a8feffe2 77 // angle += gyro_value * .01;
szh66 5:5d34a8feffe2 78 wait(.01);
szh66 5:5d34a8feffe2 79
szh66 5:5d34a8feffe2 80 serial.printf("Gyro value: %f\r\n", gyro_value);
szh66 3:34a763e93423 81
szh66 5:5d34a8feffe2 82
szh66 5:5d34a8feffe2 83 //______________
szh66 2:b78dfa2afe92 84
szh66 2:b78dfa2afe92 85
FengjunyanLi 4:86927e61e1e3 86 //serial.printf("voltage value is: %3.3f%%\r\n", Ain.read()*100.0f);
szh66 2:b78dfa2afe92 87 //serial.printf("normalized: 0x%04X \r\n", Ain.read_u16());
szh66 2:b78dfa2afe92 88
szh66 2:b78dfa2afe92 89 if(Ain.read() < 0.67f){
szh66 2:b78dfa2afe92 90 testLed = 1;
szh66 2:b78dfa2afe92 91 testLed2 = 1;
szh66 2:b78dfa2afe92 92 testLed3 = 1;
szh66 2:b78dfa2afe92 93 testLed4 = 1;
szh66 2:b78dfa2afe92 94 serial.printf("voltage is in danger area, UNPLUG\r\n");
szh66 1:11970e541ecf 95 }
szh66 2:b78dfa2afe92 96
FengjunyanLi 4:86927e61e1e3 97 int dist = getEncoderDistance();
FengjunyanLi 4:86927e61e1e3 98 //serial.printf("Encoder %d \r\n", getEncoderDistance());
szh66 0:3c59bc5c9388 99 }
szh66 0:3c59bc5c9388 100 }