Diff: main.cpp

Revision:

1:65a3a2a4f535

Parent:

0:b1290ca6c4a2

Child:

2:9dc7d5f1e910

--- a/main.cpp	Sat Feb 13 07:17:50 2016 +0000
+++ b/main.cpp	Tue Feb 16 08:09:38 2016 +0000
@@ -10,8 +10,10 @@
 CAN can_L(p30,p29);
 Serial toKeikiSerial(p28,p27);
 Serial pc(USBTX,USBRX);
-DigitalIn eruron_R(p7);
-DigitalIn eruron_L(p7);
+//DigitalIn eruron_R(p7);
+//DigitalIn eruron_L(p7);
+int eruron_R = 0;
+int eruron_L = 0;
 DigitalIn drug_R(p8);
 DigitalIn drug_L(p8);
 DigitalOut myled1(LED1);
@@ -30,7 +32,7 @@
     inputDatas_R[1] = drug_R;
     for(int i = 0; i < IMPUT_DATAS_NUM; i++){
         if(can_R.write(recmsg_R)){
-            pc.printf("Right successed!\n\r");
+            //pc.printf("Right successed!\n\r");
         }
     }
 }
@@ -49,12 +51,27 @@
         wait(0.2);
 }
 
+void servo_check(){
+    int counter = 0;
+    counter++;
+    if(counter >= 170)
+    counter = 0;
+    if(counter >= 20){
+        eruron_R = 1;
+        eruron_L = 1;
+    }
+    else{
+        eruron_R = 0;
+        eruron_L = 0;
+        }
+        }
+
 void InputAndSentControlValues_L(){
     inputDatas_L[0] = eruron_L;
     inputDatas_L[1] = drug_L;
     for(int i = 0; i < IMPUT_DATAS_NUM; i++){
         if(can_L.write(recmsg_L)){
-            pc.printf("Left successed!\n\r");
+           // pc.printf("Left successed!\n\r");
         }
     }
 }
@@ -68,14 +85,16 @@
 
 void toString_R(){
     for(int i = 0; i < YOKUTAN_DATAS_NUM; i++){
-        pc.printf("R : %i:",yokutanDatas_R[i]);
+        pc.printf("R : ");
+        pc.printf("%i:",yokutanDatas_R[i]);
     }
     pc.printf("\t");
 }
 
 void toString_L(){
     for(int i = 0; i < YOKUTAN_DATAS_NUM; i++){
-        pc.printf("L : %i:",yokutanDatas_L[i]);
+        pc.printf("L : ");
+        pc.printf("%i:",yokutanDatas_L[i]);
     }
     pc.printf("\n\r");
 }
@@ -103,6 +122,7 @@
 int main(){
     init();
     while(1){
+        servo_check();
         InputAndSentControlValues_R();
         InputAndSentControlValues_L();
         wait_us(10);