Diff: main.cpp

Branch:

XBus???

Revision:

26:f14579683f98

Parent:

25:e8bfb629e1b1

Child:

27:591cf9139ae7

--- a/main.cpp	Wed Feb 15 13:29:20 2017 +0000
+++ b/main.cpp	Thu Feb 16 16:00:11 2017 +0000
@@ -24,7 +24,6 @@
 #define ERURON_TRIM_INI_L 113   //元々95
 #define DRUG_TRIM_INI_L 110
 
-#define kXBusTx             p9
 #define kMaxServoNum        1       // 1 - 50
 #define kMaxServoPause     (sizeof(motionData) / sizeof(pauseRec))
 #define kMotionInterval     10      // flame / sec
@@ -46,7 +45,7 @@
 DigitalIn LRstatePin(p14);
 DigitalIn setTrimPin(p15);
 DigitalIn EDstatePin(p16);
-DigitalIn checkMaxDegPin(p17);
+DigitalIn setMaxDegPin(p17);
 DigitalOut debugLED(LED2);
 DigitalOut led3(LED3);
 DigitalOut led4(LED4);
@@ -69,10 +68,7 @@
 bool INA_FLAG;
 bool MPU_FLAG;
 
-//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();
@@ -80,48 +76,30 @@
 
 static const uint8_t        servoChannel = 0x01;
 
-XBusServo gXBus(kXBusTx, NC, NC, kMaxServoNum);
+XBusServo gXBus(p9, NC, NC, kMaxServoNum);
 Ticker gTimer;
 
-bool servoInit(){
+bool servoInit()
+{
     drugServo.period_ms(INIT_SERVO_PERIOD_MS);
     eruronServo.period_ms(INIT_SERVO_PERIOD_MS);
     return true;
 }
 
-//=============================================================
-// XbusIntervalHandler()
-// play motion !
-//=============================================================
-void XbusIntervalHandler()
+void sendDatas()
 {
-    uint16_t value;
-    uint16_t diff  = kMotionEndMark - kMotionMinMark;
-    value = (uint16_t)(diff * analog.read()) + kMotionMinMark;
-    gXBus.setServo(servoChannel, value);
-    gXBus.sendChannelDataPacket();
-}
-
-//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))){
+    if(can.write(CANMessage(TO_SEND_CAN_ID, toSendDatas, TO_SEND_DATAS_NUM))) {
     }
 }
 
-bool inaInit(){
-    if(!VCmonitor.isExist()){
+bool inaInit()
+{
+    if(!VCmonitor.isExist()) {
         pc.printf("VCmonitor NOT FOUND\n");
         return false;
     }
     ina_val = 0;
-    if(VCmonitor.rawRead(0x00,&ina_val) != 0){
+    if(VCmonitor.rawRead(0x00,&ina_val) != 0) {
         pc.printf("VCmonitor READ ERROR\n");
         return false;
     }
@@ -129,17 +107,30 @@
     return true;
 }
 
-XBusError init(){
-    
+XBusError initXBus()
+{
     XBusError result;
-    
-    if(!LRstatePin){
+    result = gXBus.start();
+    if (result != kXBusError_NoError) {
+        gXBus.stop();
+        return result;
+    }
+    result = gXBus.addServo(servoChannel, kXbusServoNeutral);
+    if (result != kXBusError_NoError) {
+        gXBus.stop();
+        return result;
+    }
+    return kXBusError_NoError;
+}
+
+void init()
+{
+    if(!LRstatePin) {
         eruronTrim = ERURON_TRIM_INI_L;
         drugTrim = DRUG_TRIM_INI_L;
         eruronMoveDeg = ERURON_MOVE_DEG_INI_L;
         drugMoveDeg = DRUG_MOVE_DEG_INI_L;
-    }
-    else{
+    } else {
         eruronTrim = ERURON_TRIM_INI_R;
         drugTrim = DRUG_TRIM_INI_R;
         eruronMoveDeg = ERURON_MOVE_DEG_INI_R;
@@ -152,136 +143,114 @@
     sendDatasTicker.attach(&sendDatas,SEND_DATAS_LOOP_TIME);
     // toStringTicker.attach(&toString,0.5);
     receiveDatasTicker.attach(&receiveDatas,RECEIVE_DATAS_LOOP_TIME);
-    
-    //XBus
-     // initialize XBus
-    result = gXBus.start();
-    if (result != kXBusError_NoError) {
-        gXBus.stop();
-        return result;
-    }
-    // initialize XBus Servos
-    result = gXBus.addServo(servoChannel, kXbusServoNeutral);
-    if (result != kXBusError_NoError) {
-        gXBus.stop();
-        return result;
-    }
 
-    return kXBusError_NoError;
+    initXBus();
 }
 
-void updateDatas(){
+void updateDatas()
+{
 //    if(ADXL_FLAG){
 ////        accelerometer.getOutput(acc);
 //    }
-    if(INA_FLAG){
+    if(INA_FLAG) {
         int tmp = VCmonitor.getVoltage(&V);
         tmp = VCmonitor.getCurrent(&C);
     }
-    if(MPU_FLAG){
+    if(MPU_FLAG) {
         mpu.read(MPU6050_GYRO_XOUT_H_REG, gyro_c, 6);
     }
-    for(int i = 0; i < TO_SEND_DATAS_NUM - 1; i++){
-//        toSendDatas[i] = acc[i];
+    for(int i = 0; i < TO_SEND_DATAS_NUM - 1; 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(){
-    if(can.read(recmsg)){
-        for(int i = 0; i < CONTROL_VALUES_NUM; i++){
+void receiveDatas()
+{
+    if(can.read(recmsg)) {
+        for(int i = 0; i < CONTROL_VALUES_NUM; i++) {
             controlValues[i] = recmsg.data[i];
             if(i<CONTROL_VALUES_NUM-1) floatvalues[i] = controlValues[i];
         }
         eruronfloat = *(const float *)floatvalues;
         led1 = !led1;
-        //WriteServo();
     }
 }
 
-void toString(){
-    for(int i = 0; i <CONTROL_VALUES_NUM; i++){
-        pc.printf("%d, ",controlValues[i]);
-    }
-    pc.printf("\n\r");
-}
-
-double calcPulse(int deg){
+double calcPulse(int deg)
+{
     return (0.0006+(deg/180.0)*(0.00235-0.00045));
-
 }
 
-void WriteServo(){
-    //if(debugServoPin){
-//        led3 = 1;
-//        float a = eruronAna.read()*180;
-//        float b = drugAna.read()*180;
-//        eruronServo.pulsewidth(calcPulse(eruronAna.read()*180));
-//        drugServo.pulsewidth(calcPulse(drugAna.read()*180));
-//        pc.printf("eruron:%f    drug:%f\n\r",a,b);
-//    }
-//    else{
-      //  led3 = 0;
-//        eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * (controlValues[0]-1) ));
-        drugServo.pulsewidth(calcPulse( drugTrim + drugMoveDeg * controlValues[4]));
-        eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * (eruronfloat-1) ));
-    //}
+void XbusIntervalHandler()
+{
+    uint16_t value;
+    uint16_t diff  = kMotionEndMark - kMotionMinMark;
+    value = (uint16_t)(diff * (eruronTrim  + eruronMoveDeg * (eruronfloat-1)) + kMotionMinMark);
+                       gXBus.setServo(servoChannel, value);
+                       gXBus.sendChannelDataPacket();
+}
+
+                   void WriteServo()
+{
+    drugServo.pulsewidth(calcPulse( drugTrim + drugMoveDeg * controlValues[4]));
+    eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * (eruronfloat-1) ));
 }
 
-void setTrim(){
+void setTrim()
+{
     debugLED =  1;
-    if(EDstatePin){
-    eruronTrim = eruronAna.read()*180;
-    eruronServo.pulsewidth(calcPulse(eruronTrim));
-    }
-    else{
-    drugTrim = drugAna.read()*180;
-    drugServo.pulsewidth(calcPulse(drugTrim));
+    if(EDstatePin) {
+        eruronTrim = eruronAna.read();
+        eruronServo.pulsewidth(calcPulse(eruronTrim));
+    } else {
+        drugTrim = drugAna.read()*180;
+        drugServo.pulsewidth(calcPulse(drugTrim));
     }
     //pc.printf("eruronTrim:%f    drugTrim:%f\n\r",eruronTrim,drugTrim);
-     pc.printf("eruronTrim:%f    drugTrim:%f    ",eruronTrim,drugTrim);
-      pc.printf("eMD:%f   dMD:%f    ef:%f\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat);
+    pc.printf("eruronTrim:%f    drugTrim:%f    ",eruronTrim,drugTrim);
+    pc.printf("eMD:%f   dMD:%f    ef:%f\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat);
 }
 
-void checkMaxDeg(){
+void setMaxDeg()
+{
     led4 = 1;
-    float eruronTemp = eruronAna.read()*180;
+    float eruronTemp = eruronAna.read();
     float drugTemp = drugAna.read()*180;
-    if(EDstatePin){
-    eruronServo.pulsewidth(calcPulse(eruronTemp));
-    eruronMoveDeg = eruronTemp-eruronTrim;
+    if(EDstatePin) {
+        eruronServo.pulsewidth(calcPulse(eruronTemp));
+        eruronMoveDeg = eruronTemp-eruronTrim;
+    } else {
+        drugServo.pulsewidth(calcPulse(drugTemp));
+        drugMoveDeg = drugTemp-drugTrim;
     }
-    else{
-    drugServo.pulsewidth(calcPulse(drugTemp));
-    drugMoveDeg = drugTemp-drugTrim;
-    }
-   // pc.printf("eMD:%f   dMD:%f\n\r",eruronMoveDeg,drugMoveDeg);
+    // pc.printf("eMD:%f   dMD:%f\n\r",eruronMoveDeg,drugMoveDeg);
     pc.printf("eruronTrim:%f    drugTrim:%f    ",eruronTrim,drugTrim);
     pc.printf("eMD:%f   dMD:%f    ef:%f\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat);
     wait_us(10);
 }
 
-int main(){
+int main()
+{
     init();
     XBusError result;
-    
+
     // start motion
     gTimer.attach_us(&XbusIntervalHandler, 1000000 / kMotionInterval);
-    
-    while(1){
-        while(setTrimPin){
+
+    while(1) {
+        while(setTrimPin) {
             setTrim();
         }
-        while(checkMaxDegPin){
-            checkMaxDeg();
+        while (setMaxDegPin) {
+        setMaxDeg();
         }
-      //  pc.printf("eT:%f\n\r",eruronTrim);
-      pc.printf("eruronTrim:%f    drugTrim:%f    ",eruronTrim,drugTrim);
-      pc.printf("eMD:%f   dMD:%f    ef:%f\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat);
+        //  pc.printf("eT:%f\n\r",eruronTrim);
+        pc.printf("eruronTrim:%f    drugTrim:%f    ",eruronTrim,drugTrim);
+        pc.printf("eMD:%f   dMD:%f    ef:%f\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat);
         led4 = 0;
-        
+
         debugLED = 0;
         //receiveDatas();
 //        sendDatas();