pinxian wu / Mbed 2 deprecated ROBOT

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?

UserRevisionLine numberNew 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