Diff: main.cpp

Revision:

0:944be74ce25b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Dec 31 06:17:30 2016 +0000
@@ -0,0 +1,207 @@
+#include "mbed.h"
+#include "motordriver.h"
+#include "Pulse.h"
+#include "HMC5883L.h"
+#include "ADXL345_I2C.h"
+InterruptIn encoderA(A2);
+InterruptIn encoderB(A3);
+Motor Motor_A(D10,D13,D12,1);
+Motor Motor_B(D9,D11,D8,1);
+Serial pc(SERIAL_TX, SERIAL_RX);
+PulseInOut trig(A1);
+PulseInOut echo(A5);
+BusIn IR(D3,PA_14,PA_15,D4);
+HMC5883L compass(D14, D15);
+ADXL345_I2C accelerometer(D14, D15);
+int16_t data[3] = {0};
+int readings[3]= {0};
+ /*********sonar*********************/
+ float hc_distance(void)
+ {
+   long count=0;
+   float distance=0;
+     trig.write_us(1,10);
+      count=echo.read_high_us();
+      distance= float(count*0.017);
+      return distance;
+  }
+int encoder_countB=0;
+int encoder_countA=0;
+void encoderB_counter(void)
+{
+    encoder_countB+=1;
+    }
+    
+void encoderA_counter(void)
+{
+    encoder_countA+=1;
+    }
+    
+void encoderA_reset(void)
+{
+    encoder_countA=0;
+    }
+void encoderB_reset(void)
+{
+    encoder_countB=0;
+    }
+float encoderA_length(void)
+{
+   return encoder_countA*42*3.14/12;
+    }
+float encoderB_length(void)
+{
+   return encoder_countB*42*3.14/12;
+    }    
+void go_straight(int div,float speed)
+{
+    encoderA_reset();encoderB_reset();
+    while(encoder_countA<div){
+    Motor_A.speed(speed);Motor_B.speed(speed);
+    }
+    encoderA_reset();encoderB_reset();
+    }    
+void turn_left(int i)
+{
+     encoderA_reset();encoderB_reset();
+     while(encoder_countA<i){
+     Motor_A.speed(1.0);Motor_B.speed(-1.0);
+     Motor_A.speed(1.0);Motor_B.speed(-1.0);
+     }
+      Motor_A.stop(0.5); Motor_B.stop(0.5);
+     printf("%d , %d \n",encoder_countA,encoder_countB);
+     encoderA_reset();encoderB_reset();
+}   
+void turn_right(int i)
+{
+     encoderA_reset();encoderB_reset();
+     while(encoder_countA<i){
+     Motor_A.speed(-1.0);Motor_B.speed(1.0);
+     Motor_A.speed(-1.0);Motor_B.speed(1.0);
+     }
+     Motor_A.stop(0.5); Motor_B.stop(0.5);
+     printf("%d , %d \n",encoder_countA,encoder_countB);
+     encoderA_reset();encoderB_reset();
+} 
+void new_turn_left(float i,float speed)
+{
+    float pre_angle=0.0;
+    float angle=0.0;
+    float sum=0.0;
+    pre_angle=compass.getHeadingXYDeg();
+    printf("pre = %f\n",pre_angle);
+     Motor_A.speed(1*speed);Motor_B.speed(-1*speed);
+    Motor_A.speed(1*speed);Motor_B.speed(-1*speed);
+    while(sum<i){
+        angle=compass.getHeadingXYDeg();
+        if(abs(pre_angle-angle)>=300){
+            sum+=pre_angle+360-angle;
+            }
+        else{
+            sum+=abs(pre_angle-angle);
+            }    
+         pre_angle=angle;  
+        }
+    Motor_A.stop(0.5); Motor_B.stop(0.5);
+    printf("now = %f\n",angle);
+}
+void new_turn_right(float i,float speed)
+{
+    float pre_angle=0.0;
+    float angle=0.0;
+    float sum=0.0;
+    pre_angle=compass.getHeadingXYDeg();
+    printf("pre = %f\n",pre_angle);
+    Motor_A.speed(-1*speed);Motor_B.speed(1*speed);
+    Motor_A.speed(-1*speed);Motor_B.speed(1*speed);
+    while(sum<i){
+        angle=compass.getHeadingXYDeg();
+        if(abs(pre_angle-angle)>=300){
+            sum+=pre_angle+360-angle;
+            }
+        else{
+            sum+=abs(pre_angle-angle);
+            }    
+         pre_angle=angle;  
+        }
+    Motor_A.stop(0.5); Motor_B.stop(0.5);
+    printf("now = %f\n",angle);
+}      
+void ADXL345_init(void)
+{
+    pc.printf("Starting ADXL345 test...\n");
+    wait(.001);
+    pc.printf("Device ID is: 0x%02x\n", accelerometer.getDeviceID());
+    wait(.001);
+    
+     // These are here to test whether any of the initialization fails. It will print the failure
+    if (accelerometer.setPowerControl(0x00)){
+         pc.printf("didn't intitialize power control\n"); 
+          }
+     //Full resolution, +/-16g, 4mg/LSB.
+     wait(.001);
+     
+     if(accelerometer.setDataFormatControl(0x0B)){
+        pc.printf("didn't set data format\n");
+          }
+     wait(.001);
+     
+     //3.2kHz data rate.
+     if(accelerometer.setDataRate(ADXL345_3200HZ)){
+        pc.printf("didn't set data rate\n");
+            }
+     wait(.001);
+     
+     //Measurement mode.
+     
+     if(accelerometer.setPowerControl(MeasurementMode)) {
+        pc.printf("didn't set the power control to measurement\n"); 
+          } 
+    }
+
+float tilt(void)
+{
+    float tilt_angle=0;
+    accelerometer.getOutput(readings);
+    tilt_angle=atan(static_cast<float>(readings[0])/static_cast<float>(readings[1]));
+    return tilt_angle;
+    }
+/**********************************/
+int main() {
+    
+    int IR_value;
+    double deg=0.0;
+    float ang=0.0;
+    wait(1);
+    ADXL345_init();
+    compass.init();
+    encoderB.rise(&encoderB_counter);          //interrupt B rise-trig 
+    encoderA.rise(&encoderA_counter);          //interrupt A rise-trig
+    printf("begin\n");
+    
+ while(1)
+ {
+   /* IR_value = IR.read();
+    printf("%x\n",IR_value);
+    deg=compass.getHeadingXYDeg();
+    printf("deg = %f\n",deg);*/
+    
+    accelerometer.getOutput(readings);
+    printf("tilt = %f\n",tilt());
+   // printf("%d %d %d\n",readings[0],readings[1],readings[2]);
+    wait(0.5);
+    
+    /*
+    scanf("%f",&ang);
+    if(ang>0)
+    {
+         new_turn_left(abs(ang),0.7);
+        }
+   else{
+         new_turn_right(abs(ang),0.7);
+    }
+    */
+    }
+}
+
+ 
\ No newline at end of file