Diff: main.cpp

Revision:

3:4417217b4f66

Parent:

2:7fcb4f970a02

Child:

4:450cafd95ac3

Child:

5:7459a428e16e

Child:

6:a839ccbf7f9a

--- a/main.cpp	Tue Feb 16 09:49:50 2016 +0000
+++ b/main.cpp	Wed Feb 17 01:56:21 2016 +0000
@@ -4,7 +4,7 @@
 #include "INA226.hpp"
 #define TO_SEND_DATAS_NUM 5
 #define INIT_SERVO_PERIOD_MS 20
-#define WAIT_LOOP_TIME 1
+#define WAIT_LOOP_TIME 0.2
 #define CONTROL_VALUES_NUM 2
 #define TO_SEND_CAN_ID 100
 
@@ -14,8 +14,8 @@
 ADXL345_I2C accelerometer(p9, p10);
 I2C ina226_i2c(p28,p27);
 INA226 VCmonitor(ina226_i2c);
-PwmOut servo1(p21);
-PwmOut servo2(p22);
+PwmOut servo1(p22);
+PwmOut servo2(p23);
 DigitalOut led1(LED1);
 AnalogIn a(p20);
 AnalogIn b(p19);
@@ -63,7 +63,7 @@
 }
 
 void init(){
-    pc.printf("Receiver\n\r");
+   // pc.printf("Receiver\n\r");
     SERVO_FLAG = servoInit();
     ADXL_FLAG = adxlInit();
     INA_FLAG = inaInit();
@@ -79,7 +79,9 @@
     }
     for(int i = 0; i < 3; i++){
         toSendDatas[i] = acc[i];
+        //pc.printf("%d\t",acc[i]);
     }
+    //pc.printf("\n");
     toSendDatas[3] = (char)(V/100);
     toSendDatas[4] = (char)(C/0100);
 }
@@ -87,11 +89,9 @@
 void receiveDatas(){
     if(can.read(recmsg)){
         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]);
+            controlValues[i] = recmsg.data[i];
+         //   pc.printf("%d:%d    ",i,recmsg.data[i]);
         }
-        pc.printf("\n\r");
         led1 = !led1;
     }
 }
@@ -100,49 +100,52 @@
     //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);
+       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");
+    //for(int i = 0; i <CONTROL_VALUES_NUM; i++){
+//      //  pc.printf("%d, ",controlValues[i]);
+//        
+//    }
+    //pc.printf("\n\r");
+//    pc.printf("%f",a.read());
+    //pc.printf("\n\r");
 }
 
 void sendDatas(){
-    if(can.write(CANMessage(TO_SEND_CAN_ID, toSendDatas, TO_SEND_DATAS_NUM)))
-        pc.printf("resend suc\n\r");   
+   if(can.write(CANMessage(TO_SEND_CAN_ID, toSendDatas, TO_SEND_DATAS_NUM))){
+    //
+      //  pc.printf("resend suc\n\r");   
 }
-
+}
 double calcPulse(int deg){
     return (0.00093+(deg/180.0)*(0.00235-0.00077));
 }
 
 void WriteServo(){
-    //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());
+    if(controlValues[0] == (char)1){
+        servo1.pulsewidth(calcPulse(90));
+    } 
+    else{
+        servo1.pulsewidth(calcPulse(45));
+    }
+    if(controlValues[1] == (char)1){
+        servo2.pulsewidth(calcPulse(90));
+    } 
+    else{
+        servo2.pulsewidth(calcPulse(45));
+    }
+    //servo1.pulsewidth(calcPulse(a.read()*170));
+//    servo2.pulsewidth(calcPulse(b.read()*170));
+    //pc.printf("%f", a.read());
 }
 
 int main(){
     init();
     while(1){
         receiveDatas();
-        updateDatas();
         WriteServo();
+           updateDatas();
         toString();
         sendDatas();
         wait(WAIT_LOOP_TIME);