zhouhang shao
/
test
test
main.cpp@6:9f698d1b2996, 2017-05-13 (annotated)
- Committer:
- kolanery
- Date:
- Sat May 13 19:42:23 2017 +0000
- Revision:
- 6:9f698d1b2996
- Parent:
- 5:5d34a8feffe2
update test case
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
szh66 | 0:3c59bc5c9388 | 1 | #include "mbed.h" |
kolanery | 6:9f698d1b2996 | 2 | //#include "IRSensor.h" |
kolanery | 6:9f698d1b2996 | 3 | //#include "encoder.h" |
kolanery | 6:9f698d1b2996 | 4 | #include "left_motor.h" |
kolanery | 6:9f698d1b2996 | 5 | |
kolanery | 6:9f698d1b2996 | 6 | /* |
kolanery | 6:9f698d1b2996 | 7 | Serial serial (PA_9, PA_10); |
kolanery | 6:9f698d1b2996 | 8 | |
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 |
kolanery | 6:9f698d1b2996 | 15 | */ |
kolanery | 6:9f698d1b2996 | 16 | |
kolanery | 6:9f698d1b2996 | 17 | |
kolanery | 6:9f698d1b2996 | 18 | DigitalOut testLed(PB_12); |
kolanery | 6:9f698d1b2996 | 19 | DigitalOut testLed2(PB_13); |
kolanery | 6:9f698d1b2996 | 20 | DigitalOut testLed3(PB_14); |
kolanery | 6:9f698d1b2996 | 21 | DigitalOut testLed4(PB_15); |
kolanery | 6:9f698d1b2996 | 22 | |
kolanery | 6:9f698d1b2996 | 23 | /* |
kolanery | 6:9f698d1b2996 | 24 | AnalogIn Ain(PA_3); |
kolanery | 6:9f698d1b2996 | 25 | AnalogIn gyro(PC_1); |
kolanery | 6:9f698d1b2996 | 26 | DigitalOut gyro_sleep(PC_2); |
kolanery | 6:9f698d1b2996 | 27 | |
szh66 | 5:5d34a8feffe2 | 28 | |
kolanery | 6:9f698d1b2996 | 29 | */ |
kolanery | 6:9f698d1b2996 | 30 | |
szh66 | 2:b78dfa2afe92 | 31 | //Motor leftMotor(PB_7, PB_6); |
FengjunyanLi | 4:86927e61e1e3 | 32 | //Motor rightMotor(PB_9, PB_8); |
kolanery | 6:9f698d1b2996 | 33 | /* |
kolanery | 6:9f698d1b2996 | 34 | |
kolanery | 6:9f698d1b2996 | 35 | |
kolanery | 6:9f698d1b2996 | 36 | |
kolanery | 6:9f698d1b2996 | 37 | */ |
kolanery | 6:9f698d1b2996 | 38 | |
szh66 | 3:34a763e93423 | 39 | // pwm |
kolanery | 6:9f698d1b2996 | 40 | /* |
szh66 | 3:34a763e93423 | 41 | PwmOut motor_pwm_left_sig(PB_7); |
szh66 | 3:34a763e93423 | 42 | DigitalOut dir_left(PB_6); |
szh66 | 3:34a763e93423 | 43 | //// dir |
szh66 | 3:34a763e93423 | 44 | PwmOut motor_pwm_right_sig(PB_9); |
szh66 | 3:34a763e93423 | 45 | DigitalOut dir_right(PB_8); |
kolanery | 6:9f698d1b2996 | 46 | */ |
kolanery | 6:9f698d1b2996 | 47 | /* |
FengjunyanLi | 4:86927e61e1e3 | 48 | |
szh66 | 3:34a763e93423 | 49 | Encoder leftEncoder(PC_9, PC_8); |
szh66 | 0:3c59bc5c9388 | 50 | |
kolanery | 6:9f698d1b2996 | 51 | |
kolanery | 6:9f698d1b2996 | 52 | void battery_helper() { |
kolanery | 6:9f698d1b2996 | 53 | if(Ain.read() < 0.67f){ |
szh66 | 2:b78dfa2afe92 | 54 | testLed = 1; |
szh66 | 2:b78dfa2afe92 | 55 | testLed2 = 1; |
szh66 | 2:b78dfa2afe92 | 56 | testLed3 = 1; |
szh66 | 2:b78dfa2afe92 | 57 | testLed4 = 1; |
szh66 | 2:b78dfa2afe92 | 58 | serial.printf("voltage is in danger area, UNPLUG\r\n"); |
kolanery | 6:9f698d1b2996 | 59 | } |
kolanery | 6:9f698d1b2996 | 60 | } |
kolanery | 6:9f698d1b2996 | 61 | */ |
kolanery | 6:9f698d1b2996 | 62 | |
kolanery | 6:9f698d1b2996 | 63 | /* |
kolanery | 6:9f698d1b2996 | 64 | void toggle() { |
kolanery | 6:9f698d1b2996 | 65 | testLed2 = 1; |
kolanery | 6:9f698d1b2996 | 66 | testLed3 = 1; |
kolanery | 6:9f698d1b2996 | 67 | |
kolanery | 6:9f698d1b2996 | 68 | leftMotor.speed(0.2f); |
kolanery | 6:9f698d1b2996 | 69 | rightMotor.speed(0.2f); |
kolanery | 6:9f698d1b2996 | 70 | |
kolanery | 6:9f698d1b2996 | 71 | |
kolanery | 6:9f698d1b2996 | 72 | motor_pwm_left_sig.period(0.001f); |
kolanery | 6:9f698d1b2996 | 73 | motor_pwm_left_sig.write(0.2f); |
kolanery | 6:9f698d1b2996 | 74 | motor_pwm_right_sig.period(0.001f); |
kolanery | 6:9f698d1b2996 | 75 | motor_pwm_right_sig.write(0.6f); |
kolanery | 6:9f698d1b2996 | 76 | |
kolanery | 6:9f698d1b2996 | 77 | wait(5); |
kolanery | 6:9f698d1b2996 | 78 | motor_pwm_right_sig = 0; |
kolanery | 6:9f698d1b2996 | 79 | motor_pwm_left_sig = 0; |
kolanery | 6:9f698d1b2996 | 80 | |
kolanery | 6:9f698d1b2996 | 81 | |
kolanery | 6:9f698d1b2996 | 82 | } */ |
kolanery | 6:9f698d1b2996 | 83 | |
kolanery | 6:9f698d1b2996 | 84 | int main () |
kolanery | 6:9f698d1b2996 | 85 | { |
kolanery | 6:9f698d1b2996 | 86 | //serial.printf ("Start Program\n"); |
kolanery | 6:9f698d1b2996 | 87 | testLed2 = 1; |
kolanery | 6:9f698d1b2996 | 88 | testLed3 = 1; |
kolanery | 6:9f698d1b2996 | 89 | |
kolanery | 6:9f698d1b2996 | 90 | LeftMotor * motor = new LeftMotor(); |
kolanery | 6:9f698d1b2996 | 91 | motor->inv_dir(0); |
kolanery | 6:9f698d1b2996 | 92 | motor->speed(0.9f); |
kolanery | 6:9f698d1b2996 | 93 | |
kolanery | 6:9f698d1b2996 | 94 | |
kolanery | 6:9f698d1b2996 | 95 | /* |
kolanery | 6:9f698d1b2996 | 96 | |
kolanery | 6:9f698d1b2996 | 97 | PwmOut motor_pwm_left_sig(PB_7); |
kolanery | 6:9f698d1b2996 | 98 | DigitalOut dir_left(PB_6); |
kolanery | 6:9f698d1b2996 | 99 | //// dir |
kolanery | 6:9f698d1b2996 | 100 | PwmOut motor_pwm_right_sig(PB_9); |
kolanery | 6:9f698d1b2996 | 101 | DigitalOut dir_right(PB_8); |
kolanery | 6:9f698d1b2996 | 102 | |
kolanery | 6:9f698d1b2996 | 103 | */ |
kolanery | 6:9f698d1b2996 | 104 | |
kolanery | 6:9f698d1b2996 | 105 | /* |
kolanery | 6:9f698d1b2996 | 106 | Motor * leftMotor = new Motor(PB_7, PB_6); |
kolanery | 6:9f698d1b2996 | 107 | Motor * rightMotor = new Motor(PB_9, PB_8); |
kolanery | 6:9f698d1b2996 | 108 | |
kolanery | 6:9f698d1b2996 | 109 | leftMotor->speed(0.2f); |
kolanery | 6:9f698d1b2996 | 110 | rightMotor->speed(0.2f); |
kolanery | 6:9f698d1b2996 | 111 | */ |
kolanery | 6:9f698d1b2996 | 112 | |
kolanery | 6:9f698d1b2996 | 113 | //leftMotor = 0.2f; |
kolanery | 6:9f698d1b2996 | 114 | //rightMotor = 0.2f; |
kolanery | 6:9f698d1b2996 | 115 | |
kolanery | 6:9f698d1b2996 | 116 | /* |
kolanery | 6:9f698d1b2996 | 117 | wait(3); |
kolanery | 6:9f698d1b2996 | 118 | toggle(); |
kolanery | 6:9f698d1b2996 | 119 | |
kolanery | 6:9f698d1b2996 | 120 | wait(3); |
kolanery | 6:9f698d1b2996 | 121 | toggle(); |
kolanery | 6:9f698d1b2996 | 122 | |
kolanery | 6:9f698d1b2996 | 123 | wait(3); |
kolanery | 6:9f698d1b2996 | 124 | toggle(); |
kolanery | 6:9f698d1b2996 | 125 | |
kolanery | 6:9f698d1b2996 | 126 | wait(3); |
kolanery | 6:9f698d1b2996 | 127 | toggle(); |
kolanery | 6:9f698d1b2996 | 128 | */ |
kolanery | 6:9f698d1b2996 | 129 | |
kolanery | 6:9f698d1b2996 | 130 | //double angle |
kolanery | 6:9f698d1b2996 | 131 | /* |
kolanery | 6:9f698d1b2996 | 132 | while (1) |
kolanery | 6:9f698d1b2996 | 133 | { |
kolanery | 6:9f698d1b2996 | 134 | battery_helper(); |
kolanery | 6:9f698d1b2996 | 135 | |
kolanery | 6:9f698d1b2996 | 136 | testLed2 = 1; |
kolanery | 6:9f698d1b2996 | 137 | testLed3 = 1; |
kolanery | 6:9f698d1b2996 | 138 | |
kolanery | 6:9f698d1b2996 | 139 | // wait (0.2); |
kolanery | 6:9f698d1b2996 | 140 | // testLed2 = 0; |
kolanery | 6:9f698d1b2996 | 141 | // testLed3 = 0; |
kolanery | 6:9f698d1b2996 | 142 | //wait (0.2); |
kolanery | 6:9f698d1b2996 | 143 | |
kolanery | 6:9f698d1b2996 | 144 | |
kolanery | 6:9f698d1b2996 | 145 | motor_pwm_left_sig.period(0.001f); |
kolanery | 6:9f698d1b2996 | 146 | motor_pwm_left_sig.write(0.2f); |
kolanery | 6:9f698d1b2996 | 147 | motor_pwm_right_sig.period(0.001f); |
kolanery | 6:9f698d1b2996 | 148 | motor_pwm_right_sig.write(0.6f); |
kolanery | 6:9f698d1b2996 | 149 | dir_left = 0; |
kolanery | 6:9f698d1b2996 | 150 | dir_right = 1; |
kolanery | 6:9f698d1b2996 | 151 | |
kolanery | 6:9f698d1b2996 | 152 | wait(2); |
kolanery | 6:9f698d1b2996 | 153 | |
kolanery | 6:9f698d1b2996 | 154 | motor_pwm_right_sig = 0; |
kolanery | 6:9f698d1b2996 | 155 | dir_right = 0; |
kolanery | 6:9f698d1b2996 | 156 | motor_pwm_left_sig = 0; |
kolanery | 6:9f698d1b2996 | 157 | |
szh66 | 2:b78dfa2afe92 | 158 | |
FengjunyanLi | 4:86927e61e1e3 | 159 | int dist = getEncoderDistance(); |
FengjunyanLi | 4:86927e61e1e3 | 160 | //serial.printf("Encoder %d \r\n", getEncoderDistance()); |
kolanery | 6:9f698d1b2996 | 161 | } */ |
kolanery | 6:9f698d1b2996 | 162 | } |
kolanery | 6:9f698d1b2996 | 163 | |
kolanery | 6:9f698d1b2996 | 164 | |
kolanery | 6:9f698d1b2996 | 165 | /* |
kolanery | 6:9f698d1b2996 | 166 | void motor_tester() { |
kolanery | 6:9f698d1b2996 | 167 | |
kolanery | 6:9f698d1b2996 | 168 | } |
kolanery | 6:9f698d1b2996 | 169 | |
kolanery | 6:9f698d1b2996 | 170 | void sensor_tester() { |
kolanery | 6:9f698d1b2996 | 171 | // serial.printf("Left sensor: %f\r\n", testIR_LEFT.readIR()); |
kolanery | 6:9f698d1b2996 | 172 | // serial.printf("Left Diagnal %f\r\n", testIR_DIAG_LEFT.readIR()); |
kolanery | 6:9f698d1b2996 | 173 | serial.printf("TOP LEFT %f\r\n", testIR_TOP1.readIR()); |
kolanery | 6:9f698d1b2996 | 174 | serial.printf("TOP RIGHT %f\r\n", testIR_TOP2.readIR()); |
kolanery | 6:9f698d1b2996 | 175 | // serial.printf("Right Diagnal %f\r\n", testIR_DIAG_RIGHT.readIR()); |
kolanery | 6:9f698d1b2996 | 176 | // serial.printf("Right sensor: %f\r\n", testIR_RIGHT.readIR()); |
kolanery | 6:9f698d1b2996 | 177 | wait(1); |
kolanery | 6:9f698d1b2996 | 178 | |
kolanery | 6:9f698d1b2996 | 179 | } |
kolanery | 6:9f698d1b2996 | 180 | |
kolanery | 6:9f698d1b2996 | 181 | void voltage_tester() { |
kolanery | 6:9f698d1b2996 | 182 | serial.printf("voltage value is: %3.3f%%\r\n", Ain.read()*100.0f); |
kolanery | 6:9f698d1b2996 | 183 | serial.printf("normalized: 0x%04X \r\n", Ain.read_u16()); |
kolanery | 6:9f698d1b2996 | 184 | } |
kolanery | 6:9f698d1b2996 | 185 | |
kolanery | 6:9f698d1b2996 | 186 | void gyro_tester() { |
kolanery | 6:9f698d1b2996 | 187 | double offset = .4451; |
kolanery | 6:9f698d1b2996 | 188 | gyro_sleep = 1; |
kolanery | 6:9f698d1b2996 | 189 | double gyro_value = (gyro.read() - offset) * .67 / offset * 600; // Offset of .4455 measured experimentally |
kolanery | 6:9f698d1b2996 | 190 | angle += gyro_value * .01; |
kolanery | 6:9f698d1b2996 | 191 | wait(.01); |
kolanery | 6:9f698d1b2996 | 192 | serial.printf("Gyro value: %f\r\n", gyro_value); |
kolanery | 6:9f698d1b2996 | 193 | } */ |