Diff: main.cpp

Branch:

mpu????????

Revision:

61:988e3f4280ac

Parent:

60:a45dc19a6001

Child:

62:1db967d29809

--- a/main.cpp	Wed Jun 07 14:13:53 2017 +0000
+++ b/main.cpp	Wed Jun 07 15:14:10 2017 +0000
@@ -59,7 +59,6 @@
 I2C i2c_mpu(p9,p10);
 
 char toSendDatas[TO_SEND_DATAS_NUM];
-char controlValues[sizeof(float) +3];//0~3:eruruon,4( sizeof(float)で指定してください ):drug
 char floatValues[10];
 float eruronTrim;
 float drugTrim;
@@ -71,11 +70,11 @@
 bool SERVO_FLAG;
 bool INA_FLAG;
 bool MPU_FLAG;
-uint16_t XbusValue;
 int gyroX;
 int gyroY;
 int gyroZ;
 float sum = 0;
+float drugInput = 0.0;
 uint32_t sumCount = 0;
 
 char gyro_c[6] = {0,0,0,0,0,0};
@@ -186,8 +185,7 @@
 //動かしたいエレボンの角度から、動かしたいサーボホーンの角度を得る。
 double ConvertDeg(double servo)
 {
-    double result = 0.0003*pow(servo,3)+0.0039*pow(servo,2)+1.746*servo - 0.0105;
-    return result;
+    return 0.0003*pow(servo,3)+0.0039*pow(servo,2)+1.746*servo - 0.0105;
 }
 
 //ホーンを動かしたい角度から、変化させるアナログ値の幅を得る。3度変化
@@ -199,8 +197,7 @@
 double GetFloatByErebon(double erebonDeg)
 {
     double servoDeg = ConvertDeg(erebonDeg);
-    double FirstMoveDeg = GetValueByHorn(servoDeg);
-    return FirstMoveDeg;
+    return GetValueByHorn(servoDeg);
 }
 
 void init()
@@ -221,8 +218,6 @@
     sendDatasTicker.attach(&sendDatas,SEND_DATAS_LOOP_TIME);
     // toStringTicker.attach(&toString,0.5);
     receiveDatasTicker.attach(&receiveDatas,RECEIVE_DATAS_LOOP_TIME);
-
-    //  initXBus();
 }
 
 void updateDatas()
@@ -241,12 +236,11 @@
 
 void receiveDatas()
 {
-    if(can.read(recmsg)) {
-        for(int i = 0; i < 6; i++) {
+    if(can.read(recmsg)) {              //ここの中でpc.printfすると重すぎて固まるので注意
+        for(int i = 0; i < 5; i++) {
             floatValues[i] = recmsg.data[i];
-            //   controlValues[i] = recmsg.data[i];
-            //   if(i<sizeof(float)) floatValues[i] = controlValues[i];
         }
+        drugInput = recmsg.data[5]- '0';
         eruronfloat = atof(floatValues);
         led1 = !led1;
     }
@@ -255,34 +249,18 @@
 double calcPulse(float analog)
 {
     return (0.0006 + (analog)*(0.00240-0.00060) );
-//    double min = 0.0006;
-//    double max = 0.00240;
-//    if(analog >= max)
-//        analog = max;
-//    else if(analog <= min)
-//        analog = min;
-//
-//    return (min+(analog)*(max-min));
-    /*
-        int start=510, end=2390;
-    while(1) {
-        //    pc.printf("%f\n\r",(start + (double)(end - start) * analogIn.read()));
-        pc.printf("%f\n\r",analogIn.read());
-        pwm.pulsewidth_us(start + (double)(end - start) * analogIn.read());
-    */
 }
 
 void WriteServo()
 {
-    for(int i = 0; i< 10; i++) {
-        pc.printf("%c",floatValues[i]);
-    }
-    pc.printf(" : %f",eruronfloat);
-    pc.printf("\n\r");
-    drugServo.pulsewidth(calcPulse(drugTrim + drugMoveDeg *(float)controlValues[sizeof(float) + 2]));
+   // for(int i = 0; i< 10; i++) {
+//        pc.printf("%c",floatValues[i]);
+//    }
+//    pc.printf(" : %f",eruronfloat);
+//    pc.printf("\n\r");
+    drugServo.pulsewidth(calcPulse(drugTrim + drugMoveDeg *drugInput));
     eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * eruronfloat));
-    //pc.printf("WriteNum:%10f    ef:%10d  \n\r",calcPulse( eruronTrim + eruronMoveDeg * eruronfloat),eruronfloat);
-    // pc.printf("drValue::%f   ei::%f\n\r",drugTrim + drugMoveDeg *(float)controlValues[sizeof(float) + 2]);
+    pc.printf("ef:%10f  %5f\n\r",eruronfloat,drugInput);
 }
 
 void setTrim()