Revision 57:f6226219e93d, committed 2017-10-21

Comitter:

tsumagari

Date:

Sat Oct 21 06:52:43 2017 +0000

Branch:

?????

Parent:

56:b39ffd94aa54

Child:

58:0b4b842149de

Commit message:

servoOff????????can?????????

Changed in this revision

--- a/main.cpp	Fri Jun 30 10:03:09 2017 +0000
+++ b/main.cpp	Sat Oct 21 06:52:43 2017 +0000
@@ -5,7 +5,7 @@
 //#include "rtos.h"
 #define WAIT_LOOP_TIME 0.001
 #define YOKUTAN_DATAS_NUM 7
-#define INPUT_DATAS_NUM 7 //ここは1倍とまでしかCANでは一度に送れないため、8以下。そして、翼端コードと数字を合わせる必要あり。
+#define INPUT_DATAS_NUM 8 //ここは8バイトまでしかCANでは一度に送れないため、8以下。そして、翼端コードと数字を合わせる必要あり。
 #define SEND_DATAS_CAN_ID 100
 #define SEND_DATAS_TIME 0.5
 #define THRESHOLD_OH_VALUE 0.14
@@ -39,6 +39,7 @@
 AnalogIn pitchPin(p18);
 DigitalIn drug_R(p14);
 DigitalIn drug_L(p17);
+DigitalIn servoOff(p26);
 DigitalOut led1(LED1);
 DigitalOut led2(LED2);
 DigitalOut led4(LED4);
@@ -67,8 +68,8 @@
 //}
 void SyntheRollAndPitch(double pitch,double roll,float* inputR,float* inputL)
 {
-    *inputR =  (pitch * PITCH_PER_ROLL + roll)/(PITCH_PER_ROLL + 1.0);
-    *inputL =  (pitch * PITCH_PER_ROLL - roll)/(PITCH_PER_ROLL + 1.0);
+    *inputR =  (pitch * PITCH_PER_ROLL - roll)/(PITCH_PER_ROLL + 1.0);
+    *inputL =  (-pitch * PITCH_PER_ROLL - roll)/(PITCH_PER_ROLL + 1.0);
 }
 
 void InputControlValues()
@@ -78,10 +79,11 @@
     
     float inputR = 0.0;
     float inputL = 0.0;
-
+    
+    int servoV = servoOff;
     SyntheRollAndPitch(normedPitch,normedRoll,&inputR,&inputL);
-    sprintf(inputDatas_R,"%5.2f%d",inputR,(int)drug_R);
-    sprintf(inputDatas_L,"%5.2f%d",inputL,(int)drug_L);
+    sprintf(inputDatas_R,"%5.2f%d%d",inputR,(int)drug_R,servoV);
+    sprintf(inputDatas_L,"%5.2f%d%d",inputL,(int)drug_L,servoV);
     pc.printf("inR:%5.2f inL:%5.2f nrmR:%5.2f nrmL:%5.2f sMax:%5.2f sMin:%5.2f\n\r",inputR,inputL,normedPitch,normedRoll,rollIC.shiftedMax,rollIC.shiftedMin);
 
     //inputDatas_R[sizeof(float)] = (char)drug_R;
@@ -120,28 +122,14 @@
    // resetPin.mode(PullDown);
 //-----------------------------------------------------------
 //    sendDatasTicker.attach(SendDatas,SEND_DATAS_TIME);
+    servoOff.mode(PullDown);
     for(int i = 0; i < INPUT_DATAS_NUM; i++) {
         inputDatas_R[i] = 0;
         inputDatas_L[i] = 0;
     }
 }
 
-void receiveDatas()
-{
-    if(can_R.read(recmsg_R)) {
-        for(int i = 0; i < recmsg_R.len; i++) {
-            yokutanDatas_R[i] = recmsg_R.data[i];
-        }
-        led1 = !led1;
-    }
-    if(can_L.read(recmsg_L)) {
-        for(int i = 0; i < recmsg_L.len; i++) {
-            yokutanDatas_L[i] = recmsg_L.data[i];
-            led3 = !led3;
-        }
-        led2 = !led2;
-    }
-}
+
 
 int main()
 {
@@ -153,7 +141,7 @@
 
         InputControlValues();
         wait_us(5);
-        receiveDatas();
+//        receiveDatas();
         SendDatas();
         wait(WAIT_LOOP_TIME);
     }