Diff: main.cpp

Revision:

22:b38bc18ec3a1

Parent:

21:8a621c81507d

Child:

23:d551db88df65

Child:

24:d416722b4aad

--- a/main.cpp	Thu Oct 27 15:54:44 2016 +0000
+++ b/main.cpp	Sat Jan 28 02:28:06 2017 +0000
@@ -1,9 +1,10 @@
 //翼端can program
 #include "mbed.h"
-#include "ADXL345_I2C.h"
+//#include "ADXL345_I2C.h"
+#include "MPU6050.h"
 #include "INA226.hpp"
 
-#define TO_SEND_DATAS_NUM 4
+#define TO_SEND_DATAS_NUM 7
 #define INIT_SERVO_PERIOD_MS 20
 #define WAIT_LOOP_TIME 0.02
 #define CONTROL_VALUES_NUM 2
@@ -25,7 +26,8 @@
 CAN can(p30,p29);
 CANMessage recmsg;
 Serial pc(USBTX,USBRX);
-ADXL345_I2C accelerometer(p9, p10);
+//ADXL345_I2C accelerometer(p9, p10);
+MPU6050 mpu(p9,p10);
 I2C ina226_i2c(p28,p27);
 INA226 VCmonitor(ina226_i2c);
 PwmOut drugServo(p22);
@@ -41,7 +43,7 @@
 DigitalOut led3(LED3);
 DigitalOut led4(LED4);
 Ticker sendDatasTicker;
-Ticker toStringTicker;
+//Ticker toStringTicker;
 Ticker receiveDatasTicker;
 
 char toSendDatas[TO_SEND_DATAS_NUM];
@@ -56,10 +58,12 @@
 bool SERVO_FLAG;
 bool ADXL_FLAG;
 bool INA_FLAG;
+bool MPU_FLAG;
 
-int acc[3] = {0,0,0};
-char acc_mean[3][ADXL_MEAN_NUM];
-int adxl_mean_counter = 0;
+//int16_t acc[3] = {0,0,0};
+char gyro_c[6] = {0,0,0,0,0,0};
+//char acc_mean[3][ADXL_MEAN_NUM];
+//int adxl_mean_counter = 0;
 
 void toString();
 void receiveDatas();
@@ -71,13 +75,13 @@
     return true;
 }
 
-bool adxlInit(){
-    accelerometer.setPowerControl(0x00);
-    accelerometer.setDataFormatControl(0x0B);
-    accelerometer.setDataRate(ADXL345_3200HZ);
-    accelerometer.setPowerControl(0x08);
-    return true;
-}
+//bool adxlInit(){
+//    accelerometer.setPowerControl(0x00);
+//    accelerometer.setDataFormatControl(0x0B);
+//    accelerometer.setDataRate(ADXL345_3200HZ);
+//    accelerometer.setPowerControl(0x08);
+//    return true;
+//}
 
 void sendDatas(){
    if(can.write(CANMessage(TO_SEND_CAN_ID, toSendDatas, TO_SEND_DATAS_NUM))){
@@ -112,7 +116,8 @@
         drugMoveDeg =DRUG_MOVE_DEG_INI_R;
     }
     SERVO_FLAG = servoInit();
-    ADXL_FLAG = adxlInit();
+//    ADXL_FLAG = adxlInit();
+    MPU_FLAG = mpu.testConnection();
     INA_FLAG = inaInit();
     sendDatasTicker.attach(&sendDatas,SEND_DATAS_LOOP_TIME);
     // toStringTicker.attach(&toString,0.5);
@@ -120,17 +125,22 @@
 }
 
 void updateDatas(){
-    if(ADXL_FLAG){
-        accelerometer.getOutput(acc);
-    }
+//    if(ADXL_FLAG){
+////        accelerometer.getOutput(acc);
+//    }
     if(INA_FLAG){
         int tmp = VCmonitor.getVoltage(&V);
         tmp = VCmonitor.getCurrent(&C);
     }
-    for(int i = 0; i < 3; i++){
-        toSendDatas[i] = acc[i];
+    if(MPU_FLAG){
+        mpu.read(MPU6050_GYRO_XOUT_H_REG, gyro_c, 6);
     }
-    toSendDatas[3] = (char)(V/100);
+    for(int i = 0; i < TO_SEND_DATAS_NUM - 1; i++){
+//        toSendDatas[i] = acc[i];
+        toSendDatas[i] = gyro_c[i];
+    }
+//    toSendDatas[TO_SEND_DATAS_NUM - 1] = (char)(V/100);
+    toSendDatas[TO_SEND_DATAS_NUM - 1] = (char)77;
 }
 
 void receiveDatas(){
@@ -220,8 +230,11 @@
         
         debugLED = 0;
         //receiveDatas();
+//        sendDatas();
         WriteServo();
         updateDatas();
+//      pc.printf("%6d,%6d,%6d\n\r",gyro[0],gyro[1],gyro[2]);
+        led3 = !led3;
         wait(WAIT_LOOP_TIME);
     }
 }
\ No newline at end of file