Diff: main.cpp

Revision:

2:7fcb4f970a02

Parent:

1:9cc932a16d17

Child:

3:4417217b4f66

--- a/main.cpp	Sat Feb 13 11:35:50 2016 +0000
+++ b/main.cpp	Tue Feb 16 09:49:50 2016 +0000
@@ -3,10 +3,11 @@
 #include "ADXL345_I2C.h"
 #include "INA226.hpp"
 #define TO_SEND_DATAS_NUM 5
-#define INIT_SERVO_PERIOD_MS 20000
-#define WAIT_LOOP_TIME_MS 10
+#define INIT_SERVO_PERIOD_MS 20
+#define WAIT_LOOP_TIME 1
 #define CONTROL_VALUES_NUM 2
 #define TO_SEND_CAN_ID 100
+
 CAN can(p30,p29);
 CANMessage recmsg;
 Serial pc(USBTX,USBRX);
@@ -16,6 +17,8 @@
 PwmOut servo1(p21);
 PwmOut servo2(p22);
 DigitalOut led1(LED1);
+AnalogIn a(p20);
+AnalogIn b(p19);
 
 char toSendDatas[TO_SEND_DATAS_NUM];
 char controlValues[CONTROL_VALUES_NUM];//0:eruruon,1:drug
@@ -83,24 +86,27 @@
 
 void receiveDatas(){
     if(can.read(recmsg)){
-        for(int i = 0; i < recmsg.len; i++){
-            controlValues[i] = recmsg.data[i];
+        for(int i = 0; i < CONTROL_VALUES_NUM; i++){
+            //controlValues[i] = recmsg.data[i];
+            controlValues[i] = 1 - controlValues[i];
+            //pc.printf("%d:%d    ",i,recmsg.data[i]);
         }
+        pc.printf("\n\r");
         led1 = !led1;
     }
 }
 
 void toString(){
     //for(int i = 0; i < TO_SEND_DATAS_NUM; i++){
-        pc.printf("%i:",toSendDatas[3]);
-        pc.printf("%i:",toSendDatas[4]);
-        pc.printf("%f:",V);
-        pc.printf("%f:",C);
-    //}
-    //for(int i = 0; i <CONTROL_VALUES_NUM; i++){
-//        pc.printf("%i:",controlValues[i]);
-//    }
-//    pc.printf("%d",INA_FLAG);
+    //    pc.printf("%i:",toSendDatas[3]);
+//        pc.printf("%i:",toSendDatas[4]);
+//        pc.printf("%f:",V);
+//        pc.printf("%f:",C);
+//    //}
+    for(int i = 0; i <CONTROL_VALUES_NUM; i++){
+        pc.printf("%d:%d, ",i,controlValues[i]);
+    }
+    pc.printf("%d",INA_FLAG);
     pc.printf("\n\r");
 }
 
@@ -114,12 +120,21 @@
 }
 
 void WriteServo(){
-    if(controlValues[0]){
-        servo1.pulsewidth(calcPulse(90));
-    } 
-    else{
-        servo1.pulsewidth(calcPulse(45));
-    }
+    //if(controlValues[0] == 1){
+//        servo1.pulsewidth(calcPulse(90));
+//    } 
+//    else{
+//        servo1.pulsewidth(calcPulse(45));
+//    }
+//    if(controlValues[1] == 1){
+//        servo1.pulsewidth(calcPulse(90));
+//    } 
+//    else{
+//        servo1.pulsewidth(calcPulse(45));
+//    }
+    servo1.pulsewidth(calcPulse(a.read()*170));
+    servo2.pulsewidth(calcPulse(b.read()*170));
+    pc.printf("%f", a.read());
 }
 
 int main(){
@@ -130,6 +145,6 @@
         WriteServo();
         toString();
         sendDatas();
-        wait_ms(WAIT_LOOP_TIME_MS);
+        wait(WAIT_LOOP_TIME);
     }
 }
\ No newline at end of file