Diff: main.cpp

Revision:

19:f1c567cb5bb8

Parent:

17:c55a8b8a3eb2

Child:

20:e05fdab66e68

--- a/main.cpp	Wed Oct 07 07:47:49 2015 +0000
+++ b/main.cpp	Sat Oct 10 06:27:50 2015 +0000
@@ -11,6 +11,8 @@
 Serial conn(p9,p10);
 Serial pc(USBTX,USBRX);
 Serial slave(p28,p27);
+PwmOut Ll(LED1);
+PwmOut Rl(LED2);
 Servo L(p25);
 Servo R(p26);
 PID Tp(50,40000,0,0.001);
@@ -18,20 +20,42 @@
 BusOut led(p8,p13,p14,p24);
 Motor ot(p23,p19,p20);
 QEI sensort(p29,p30,NC,624);
-Motor sup1(p21,p15,p16);
-Motor sup2(p22,p17,p18);
+//Motor sup1(p21,p15,p16);
+//Motor sup2(p22,p17,p18);
+SoftPWM sup1p(p21);
+BusOut sup1d(p15,p16);
+SoftPWM sup2p(p22);
+BusOut sup2d(p17,p18);
 DigitalIn limit1(p11,PullUp);
 DigitalIn limit2(p12,PullUp);
 Timeout ai;
 char read;
 int Rs = 0,Ls = 0,d=0,su1 = 0,su2 = 0;
 int i = 0;
+void sup1_speed(float speed)
+{
+    if(speed>0){
+        sup1d = 1;
+    }else if(speed < 0){
+        sup1d =2;
+    }
+    sup1p = fabs(speed);
+}
+void sup2_speed(float speed)
+{
+    if(speed>0){
+        sup2d = 1;
+    }else if(speed < 0){
+        sup2d =2;
+    }
+    sup2p = fabs(speed);
+}
 void supply()
 {
     if(d%2){
-        sup1.speed(0.9);
+        sup1_speed(0.9);
     }else{
-        sup2.speed(0.9);
+        sup2_speed(0.9);
     }
     d++;
 }
@@ -62,20 +86,19 @@
     R = 0.5;
     wait(3);
     led= 15;
+    conn.baud(38400);
     while(1) {
         if(conn.getc() == 255) {
-            tmpread = conn.getc();
-            tmpttilt = conn.getc();
-            tro = conn.getc();
-            tlo = conn.getc();
-            if(tmpread^tmpttilt^tro^tlo == conn.getc()) {
-                ttilt = tmpttilt;
-                read = tmpread;
-                ro = (tro-117)/127.0*0.9;
-                lo = (tlo-127)/127.0*0.9;
-            }
+            read = conn.getc();
+            ttilt = conn.getc()-30;
+            ro = (conn.getc()-127)/127.0*0.9;
+            lo = (conn.getc()-127)/127.0*0.9;
         }
-        if((read>>2)%2 && i == 0) {
+        if(read>4){
+            read = 0;
+        }
+        led = read;
+        if((read>>1)%2 && i == 0) {
             air = 1;
             ai.attach(&rev,shoottime);
             i = 1;
@@ -89,16 +112,16 @@
         }
         if(limit1 == 1 && su1 == 1) {
             su1 = 0;
-            sup1.speed(0);
+            sup1_speed(0);
         }
         if(limit2 == 1 && su2 == 1) {
             su2 = 0;
-            sup2.speed(0);
+            sup2_speed(0);
         }
         /*射角制御*/
         Tp.setSetPoint(ttilt);
         tilt = double(sensort.getPulses());
-        tilt = tilt*55/2684.0;
+        tilt = tilt*49.0/1745.0;
         Tp.setProcessValue(tilt);
         /*足の出力が小さい場合はゼロとする*/
         if (abs(lo) < 0.1) {
@@ -111,12 +134,12 @@
         slave.putc(read);
         /*各モーターに出力*/
         L = (ro+1.0)/2.0;
+        Ll = (ro+1.0)/2.0;
         R = 1.0-(lo+1.0)/2.0;
+        Rl = 1.0-(lo+1.0)/2.0;
         ot.speed(Tp.compute());
         //pc.printf("%f\n\r",tilt);
         //pc.printf("%d\n\r",sensort.getPulses());
         //pc.printf("%f \n\r",Tp.compute());
-        pc.printf("%d-%d\r\n",tlo,tro);
-        wait_ms(20);
     }
 }