mbed robot project
Dependencies: HCSR04 HMC5883L Motordriver Pulse mbed
main.cpp@0:944be74ce25b, 2016-12-31 (annotated)
- Committer:
- wupinxian
- Date:
- Sat Dec 31 06:17:30 2016 +0000
- Revision:
- 0:944be74ce25b
mbed ROBOT
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
wupinxian | 0:944be74ce25b | 1 | #include "mbed.h" |
wupinxian | 0:944be74ce25b | 2 | #include "motordriver.h" |
wupinxian | 0:944be74ce25b | 3 | #include "Pulse.h" |
wupinxian | 0:944be74ce25b | 4 | #include "HMC5883L.h" |
wupinxian | 0:944be74ce25b | 5 | #include "ADXL345_I2C.h" |
wupinxian | 0:944be74ce25b | 6 | InterruptIn encoderA(A2); |
wupinxian | 0:944be74ce25b | 7 | InterruptIn encoderB(A3); |
wupinxian | 0:944be74ce25b | 8 | Motor Motor_A(D10,D13,D12,1); |
wupinxian | 0:944be74ce25b | 9 | Motor Motor_B(D9,D11,D8,1); |
wupinxian | 0:944be74ce25b | 10 | Serial pc(SERIAL_TX, SERIAL_RX); |
wupinxian | 0:944be74ce25b | 11 | PulseInOut trig(A1); |
wupinxian | 0:944be74ce25b | 12 | PulseInOut echo(A5); |
wupinxian | 0:944be74ce25b | 13 | BusIn IR(D3,PA_14,PA_15,D4); |
wupinxian | 0:944be74ce25b | 14 | HMC5883L compass(D14, D15); |
wupinxian | 0:944be74ce25b | 15 | ADXL345_I2C accelerometer(D14, D15); |
wupinxian | 0:944be74ce25b | 16 | int16_t data[3] = {0}; |
wupinxian | 0:944be74ce25b | 17 | int readings[3]= {0}; |
wupinxian | 0:944be74ce25b | 18 | /*********sonar*********************/ |
wupinxian | 0:944be74ce25b | 19 | float hc_distance(void) |
wupinxian | 0:944be74ce25b | 20 | { |
wupinxian | 0:944be74ce25b | 21 | long count=0; |
wupinxian | 0:944be74ce25b | 22 | float distance=0; |
wupinxian | 0:944be74ce25b | 23 | trig.write_us(1,10); |
wupinxian | 0:944be74ce25b | 24 | count=echo.read_high_us(); |
wupinxian | 0:944be74ce25b | 25 | distance= float(count*0.017); |
wupinxian | 0:944be74ce25b | 26 | return distance; |
wupinxian | 0:944be74ce25b | 27 | } |
wupinxian | 0:944be74ce25b | 28 | int encoder_countB=0; |
wupinxian | 0:944be74ce25b | 29 | int encoder_countA=0; |
wupinxian | 0:944be74ce25b | 30 | void encoderB_counter(void) |
wupinxian | 0:944be74ce25b | 31 | { |
wupinxian | 0:944be74ce25b | 32 | encoder_countB+=1; |
wupinxian | 0:944be74ce25b | 33 | } |
wupinxian | 0:944be74ce25b | 34 | |
wupinxian | 0:944be74ce25b | 35 | void encoderA_counter(void) |
wupinxian | 0:944be74ce25b | 36 | { |
wupinxian | 0:944be74ce25b | 37 | encoder_countA+=1; |
wupinxian | 0:944be74ce25b | 38 | } |
wupinxian | 0:944be74ce25b | 39 | |
wupinxian | 0:944be74ce25b | 40 | void encoderA_reset(void) |
wupinxian | 0:944be74ce25b | 41 | { |
wupinxian | 0:944be74ce25b | 42 | encoder_countA=0; |
wupinxian | 0:944be74ce25b | 43 | } |
wupinxian | 0:944be74ce25b | 44 | void encoderB_reset(void) |
wupinxian | 0:944be74ce25b | 45 | { |
wupinxian | 0:944be74ce25b | 46 | encoder_countB=0; |
wupinxian | 0:944be74ce25b | 47 | } |
wupinxian | 0:944be74ce25b | 48 | float encoderA_length(void) |
wupinxian | 0:944be74ce25b | 49 | { |
wupinxian | 0:944be74ce25b | 50 | return encoder_countA*42*3.14/12; |
wupinxian | 0:944be74ce25b | 51 | } |
wupinxian | 0:944be74ce25b | 52 | float encoderB_length(void) |
wupinxian | 0:944be74ce25b | 53 | { |
wupinxian | 0:944be74ce25b | 54 | return encoder_countB*42*3.14/12; |
wupinxian | 0:944be74ce25b | 55 | } |
wupinxian | 0:944be74ce25b | 56 | void go_straight(int div,float speed) |
wupinxian | 0:944be74ce25b | 57 | { |
wupinxian | 0:944be74ce25b | 58 | encoderA_reset();encoderB_reset(); |
wupinxian | 0:944be74ce25b | 59 | while(encoder_countA<div){ |
wupinxian | 0:944be74ce25b | 60 | Motor_A.speed(speed);Motor_B.speed(speed); |
wupinxian | 0:944be74ce25b | 61 | } |
wupinxian | 0:944be74ce25b | 62 | encoderA_reset();encoderB_reset(); |
wupinxian | 0:944be74ce25b | 63 | } |
wupinxian | 0:944be74ce25b | 64 | void turn_left(int i) |
wupinxian | 0:944be74ce25b | 65 | { |
wupinxian | 0:944be74ce25b | 66 | encoderA_reset();encoderB_reset(); |
wupinxian | 0:944be74ce25b | 67 | while(encoder_countA<i){ |
wupinxian | 0:944be74ce25b | 68 | Motor_A.speed(1.0);Motor_B.speed(-1.0); |
wupinxian | 0:944be74ce25b | 69 | Motor_A.speed(1.0);Motor_B.speed(-1.0); |
wupinxian | 0:944be74ce25b | 70 | } |
wupinxian | 0:944be74ce25b | 71 | Motor_A.stop(0.5); Motor_B.stop(0.5); |
wupinxian | 0:944be74ce25b | 72 | printf("%d , %d \n",encoder_countA,encoder_countB); |
wupinxian | 0:944be74ce25b | 73 | encoderA_reset();encoderB_reset(); |
wupinxian | 0:944be74ce25b | 74 | } |
wupinxian | 0:944be74ce25b | 75 | void turn_right(int i) |
wupinxian | 0:944be74ce25b | 76 | { |
wupinxian | 0:944be74ce25b | 77 | encoderA_reset();encoderB_reset(); |
wupinxian | 0:944be74ce25b | 78 | while(encoder_countA<i){ |
wupinxian | 0:944be74ce25b | 79 | Motor_A.speed(-1.0);Motor_B.speed(1.0); |
wupinxian | 0:944be74ce25b | 80 | Motor_A.speed(-1.0);Motor_B.speed(1.0); |
wupinxian | 0:944be74ce25b | 81 | } |
wupinxian | 0:944be74ce25b | 82 | Motor_A.stop(0.5); Motor_B.stop(0.5); |
wupinxian | 0:944be74ce25b | 83 | printf("%d , %d \n",encoder_countA,encoder_countB); |
wupinxian | 0:944be74ce25b | 84 | encoderA_reset();encoderB_reset(); |
wupinxian | 0:944be74ce25b | 85 | } |
wupinxian | 0:944be74ce25b | 86 | void new_turn_left(float i,float speed) |
wupinxian | 0:944be74ce25b | 87 | { |
wupinxian | 0:944be74ce25b | 88 | float pre_angle=0.0; |
wupinxian | 0:944be74ce25b | 89 | float angle=0.0; |
wupinxian | 0:944be74ce25b | 90 | float sum=0.0; |
wupinxian | 0:944be74ce25b | 91 | pre_angle=compass.getHeadingXYDeg(); |
wupinxian | 0:944be74ce25b | 92 | printf("pre = %f\n",pre_angle); |
wupinxian | 0:944be74ce25b | 93 | Motor_A.speed(1*speed);Motor_B.speed(-1*speed); |
wupinxian | 0:944be74ce25b | 94 | Motor_A.speed(1*speed);Motor_B.speed(-1*speed); |
wupinxian | 0:944be74ce25b | 95 | while(sum<i){ |
wupinxian | 0:944be74ce25b | 96 | angle=compass.getHeadingXYDeg(); |
wupinxian | 0:944be74ce25b | 97 | if(abs(pre_angle-angle)>=300){ |
wupinxian | 0:944be74ce25b | 98 | sum+=pre_angle+360-angle; |
wupinxian | 0:944be74ce25b | 99 | } |
wupinxian | 0:944be74ce25b | 100 | else{ |
wupinxian | 0:944be74ce25b | 101 | sum+=abs(pre_angle-angle); |
wupinxian | 0:944be74ce25b | 102 | } |
wupinxian | 0:944be74ce25b | 103 | pre_angle=angle; |
wupinxian | 0:944be74ce25b | 104 | } |
wupinxian | 0:944be74ce25b | 105 | Motor_A.stop(0.5); Motor_B.stop(0.5); |
wupinxian | 0:944be74ce25b | 106 | printf("now = %f\n",angle); |
wupinxian | 0:944be74ce25b | 107 | } |
wupinxian | 0:944be74ce25b | 108 | void new_turn_right(float i,float speed) |
wupinxian | 0:944be74ce25b | 109 | { |
wupinxian | 0:944be74ce25b | 110 | float pre_angle=0.0; |
wupinxian | 0:944be74ce25b | 111 | float angle=0.0; |
wupinxian | 0:944be74ce25b | 112 | float sum=0.0; |
wupinxian | 0:944be74ce25b | 113 | pre_angle=compass.getHeadingXYDeg(); |
wupinxian | 0:944be74ce25b | 114 | printf("pre = %f\n",pre_angle); |
wupinxian | 0:944be74ce25b | 115 | Motor_A.speed(-1*speed);Motor_B.speed(1*speed); |
wupinxian | 0:944be74ce25b | 116 | Motor_A.speed(-1*speed);Motor_B.speed(1*speed); |
wupinxian | 0:944be74ce25b | 117 | while(sum<i){ |
wupinxian | 0:944be74ce25b | 118 | angle=compass.getHeadingXYDeg(); |
wupinxian | 0:944be74ce25b | 119 | if(abs(pre_angle-angle)>=300){ |
wupinxian | 0:944be74ce25b | 120 | sum+=pre_angle+360-angle; |
wupinxian | 0:944be74ce25b | 121 | } |
wupinxian | 0:944be74ce25b | 122 | else{ |
wupinxian | 0:944be74ce25b | 123 | sum+=abs(pre_angle-angle); |
wupinxian | 0:944be74ce25b | 124 | } |
wupinxian | 0:944be74ce25b | 125 | pre_angle=angle; |
wupinxian | 0:944be74ce25b | 126 | } |
wupinxian | 0:944be74ce25b | 127 | Motor_A.stop(0.5); Motor_B.stop(0.5); |
wupinxian | 0:944be74ce25b | 128 | printf("now = %f\n",angle); |
wupinxian | 0:944be74ce25b | 129 | } |
wupinxian | 0:944be74ce25b | 130 | void ADXL345_init(void) |
wupinxian | 0:944be74ce25b | 131 | { |
wupinxian | 0:944be74ce25b | 132 | pc.printf("Starting ADXL345 test...\n"); |
wupinxian | 0:944be74ce25b | 133 | wait(.001); |
wupinxian | 0:944be74ce25b | 134 | pc.printf("Device ID is: 0x%02x\n", accelerometer.getDeviceID()); |
wupinxian | 0:944be74ce25b | 135 | wait(.001); |
wupinxian | 0:944be74ce25b | 136 | |
wupinxian | 0:944be74ce25b | 137 | // These are here to test whether any of the initialization fails. It will print the failure |
wupinxian | 0:944be74ce25b | 138 | if (accelerometer.setPowerControl(0x00)){ |
wupinxian | 0:944be74ce25b | 139 | pc.printf("didn't intitialize power control\n"); |
wupinxian | 0:944be74ce25b | 140 | } |
wupinxian | 0:944be74ce25b | 141 | //Full resolution, +/-16g, 4mg/LSB. |
wupinxian | 0:944be74ce25b | 142 | wait(.001); |
wupinxian | 0:944be74ce25b | 143 | |
wupinxian | 0:944be74ce25b | 144 | if(accelerometer.setDataFormatControl(0x0B)){ |
wupinxian | 0:944be74ce25b | 145 | pc.printf("didn't set data format\n"); |
wupinxian | 0:944be74ce25b | 146 | } |
wupinxian | 0:944be74ce25b | 147 | wait(.001); |
wupinxian | 0:944be74ce25b | 148 | |
wupinxian | 0:944be74ce25b | 149 | //3.2kHz data rate. |
wupinxian | 0:944be74ce25b | 150 | if(accelerometer.setDataRate(ADXL345_3200HZ)){ |
wupinxian | 0:944be74ce25b | 151 | pc.printf("didn't set data rate\n"); |
wupinxian | 0:944be74ce25b | 152 | } |
wupinxian | 0:944be74ce25b | 153 | wait(.001); |
wupinxian | 0:944be74ce25b | 154 | |
wupinxian | 0:944be74ce25b | 155 | //Measurement mode. |
wupinxian | 0:944be74ce25b | 156 | |
wupinxian | 0:944be74ce25b | 157 | if(accelerometer.setPowerControl(MeasurementMode)) { |
wupinxian | 0:944be74ce25b | 158 | pc.printf("didn't set the power control to measurement\n"); |
wupinxian | 0:944be74ce25b | 159 | } |
wupinxian | 0:944be74ce25b | 160 | } |
wupinxian | 0:944be74ce25b | 161 | |
wupinxian | 0:944be74ce25b | 162 | float tilt(void) |
wupinxian | 0:944be74ce25b | 163 | { |
wupinxian | 0:944be74ce25b | 164 | float tilt_angle=0; |
wupinxian | 0:944be74ce25b | 165 | accelerometer.getOutput(readings); |
wupinxian | 0:944be74ce25b | 166 | tilt_angle=atan(static_cast<float>(readings[0])/static_cast<float>(readings[1])); |
wupinxian | 0:944be74ce25b | 167 | return tilt_angle; |
wupinxian | 0:944be74ce25b | 168 | } |
wupinxian | 0:944be74ce25b | 169 | /**********************************/ |
wupinxian | 0:944be74ce25b | 170 | int main() { |
wupinxian | 0:944be74ce25b | 171 | |
wupinxian | 0:944be74ce25b | 172 | int IR_value; |
wupinxian | 0:944be74ce25b | 173 | double deg=0.0; |
wupinxian | 0:944be74ce25b | 174 | float ang=0.0; |
wupinxian | 0:944be74ce25b | 175 | wait(1); |
wupinxian | 0:944be74ce25b | 176 | ADXL345_init(); |
wupinxian | 0:944be74ce25b | 177 | compass.init(); |
wupinxian | 0:944be74ce25b | 178 | encoderB.rise(&encoderB_counter); //interrupt B rise-trig |
wupinxian | 0:944be74ce25b | 179 | encoderA.rise(&encoderA_counter); //interrupt A rise-trig |
wupinxian | 0:944be74ce25b | 180 | printf("begin\n"); |
wupinxian | 0:944be74ce25b | 181 | |
wupinxian | 0:944be74ce25b | 182 | while(1) |
wupinxian | 0:944be74ce25b | 183 | { |
wupinxian | 0:944be74ce25b | 184 | /* IR_value = IR.read(); |
wupinxian | 0:944be74ce25b | 185 | printf("%x\n",IR_value); |
wupinxian | 0:944be74ce25b | 186 | deg=compass.getHeadingXYDeg(); |
wupinxian | 0:944be74ce25b | 187 | printf("deg = %f\n",deg);*/ |
wupinxian | 0:944be74ce25b | 188 | |
wupinxian | 0:944be74ce25b | 189 | accelerometer.getOutput(readings); |
wupinxian | 0:944be74ce25b | 190 | printf("tilt = %f\n",tilt()); |
wupinxian | 0:944be74ce25b | 191 | // printf("%d %d %d\n",readings[0],readings[1],readings[2]); |
wupinxian | 0:944be74ce25b | 192 | wait(0.5); |
wupinxian | 0:944be74ce25b | 193 | |
wupinxian | 0:944be74ce25b | 194 | /* |
wupinxian | 0:944be74ce25b | 195 | scanf("%f",&ang); |
wupinxian | 0:944be74ce25b | 196 | if(ang>0) |
wupinxian | 0:944be74ce25b | 197 | { |
wupinxian | 0:944be74ce25b | 198 | new_turn_left(abs(ang),0.7); |
wupinxian | 0:944be74ce25b | 199 | } |
wupinxian | 0:944be74ce25b | 200 | else{ |
wupinxian | 0:944be74ce25b | 201 | new_turn_right(abs(ang),0.7); |
wupinxian | 0:944be74ce25b | 202 | } |
wupinxian | 0:944be74ce25b | 203 | */ |
wupinxian | 0:944be74ce25b | 204 | } |
wupinxian | 0:944be74ce25b | 205 | } |
wupinxian | 0:944be74ce25b | 206 | |
wupinxian | 0:944be74ce25b | 207 |