Diff: main.cpp

Branch:

XBus???

Revision:

29:516a5d383488

Parent:

28:99686a3f0e86

Child:

30:00041540e23c

--- a/main.cpp	Tue Mar 07 15:48:01 2017 +0000
+++ b/main.cpp	Fri Mar 10 07:48:48 2017 +0000
@@ -1,6 +1,5 @@
 //翼端can program
 #include "mbed.h"
-//#include "ADXL345_I2C.h"
 #include "MPU6050.h"
 #include "INA226.hpp"
 #include "XBusServo.h"
@@ -10,7 +9,6 @@
 #define WAIT_LOOP_TIME 0.02
 #define CONTROL_VALUES_NUM sizeof(float) + 1
 #define TO_SEND_CAN_ID 100
-#define ADXL_MEAN_NUM 10
 #define SEND_DATAS_LOOP_TIME 0.1
 #define RECEIVE_DATAS_LOOP_TIME 0.05
 
@@ -33,12 +31,10 @@
 CAN can(p30,p29);
 CANMessage recmsg;
 Serial pc(USBTX,USBRX);
-//ADXL345_I2C accelerometer(p9, p10);
 MPU6050 mpu(p9,p10);
 I2C ina226_i2c(p28,p27);
 INA226 VCmonitor(ina226_i2c);
 PwmOut drugServo(p22);
-PwmOut eruronServo(p23);
 DigitalOut led1(LED1);
 AnalogIn drugAna(p20);
 AnalogIn eruronAna(p19);
@@ -64,7 +60,6 @@
 unsigned short ina_val;
 double V,C;
 bool SERVO_FLAG;
-bool ADXL_FLAG;
 bool INA_FLAG;
 bool MPU_FLAG;
 
@@ -82,7 +77,6 @@
 bool servoInit()
 {
     drugServo.period_ms(INIT_SERVO_PERIOD_MS);
-    eruronServo.period_ms(INIT_SERVO_PERIOD_MS);
     return true;
 }
 
@@ -137,7 +131,6 @@
         drugMoveDeg =DRUG_MOVE_DEG_INI_R;
     }
     SERVO_FLAG = servoInit();
-//    ADXL_FLAG = adxlInit();
     MPU_FLAG = mpu.testConnection();
     INA_FLAG = inaInit();
     sendDatasTicker.attach(&sendDatas,SEND_DATAS_LOOP_TIME);
@@ -149,9 +142,6 @@
 
 void updateDatas()
 {
-//    if(ADXL_FLAG){
-////        accelerometer.getOutput(acc);
-//    }
     if(INA_FLAG) {
         int tmp = VCmonitor.getVoltage(&V);
         tmp = VCmonitor.getCurrent(&C);
@@ -188,14 +178,14 @@
     uint16_t value;
     uint16_t diff  = kMotionEndMark - kMotionMinMark;
     value = (uint16_t)(diff * (eruronTrim  + eruronMoveDeg * (eruronfloat-1)) + kMotionMinMark);
-                       gXBus.setServo(servoChannel, value);
-                       gXBus.sendChannelDataPacket();
+    gXBus.setServo(servoChannel, value);
+    gXBus.sendChannelDataPacket();
 }
 
-                   void WriteServo()
+void WriteServo()
 {
+    pc.printf("%d",controlValues[4]);
     drugServo.pulsewidth(calcPulse( drugTrim + drugMoveDeg * controlValues[4]));
-    eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * (eruronfloat-1) ));
 }
 
 void setTrim()
@@ -203,7 +193,6 @@
     debugLED =  1;
     if(EDstatePin) {
         eruronTrim = eruronAna.read();
-        eruronServo.pulsewidth(calcPulse(eruronTrim));
     } else {
         drugTrim = drugAna.read()*180;
         drugServo.pulsewidth(calcPulse(drugTrim));
@@ -219,7 +208,6 @@
     float eruronTemp = eruronAna.read();
     float drugTemp = drugAna.read()*180;
     if(EDstatePin) {
-        eruronServo.pulsewidth(calcPulse(eruronTemp));
         eruronMoveDeg = eruronTemp-eruronTrim;
     } else {
         drugServo.pulsewidth(calcPulse(drugTemp));
@@ -244,7 +232,7 @@
             setTrim();
         }
         while (setMaxDegPin) {
-        setMaxDeg();
+            setMaxDeg();
         }
         //  pc.printf("eT:%f\n\r",eruronTrim);
         pc.printf("eruronTrim:%f    drugTrim:%f    ",eruronTrim,drugTrim);