Diff: main.cpp

Revision:

16:6b8766c77d29

Parent:

15:a9b36208dc32

Child:

17:c55a8b8a3eb2

Child:

18:7703c9baf008

--- a/main.cpp	Thu Oct 01 10:41:45 2015 +0000
+++ b/main.cpp	Mon Oct 05 12:02:38 2015 +0000
@@ -5,6 +5,7 @@
 #include "QEI.h"
 #include "Servo.h"
 #define RATE 0.05
+#define shoottime 0.5
 BusOut air(p5,p6);
 DigitalOut out(p7);
 Serial conn(p9,p10);
@@ -12,16 +13,13 @@
 Serial slave(p28,p27);
 Servo L(p25);
 Servo R(p26);
-//PID Tp(50,40000,0,0.001);
-PID Tp(4.,40000,0,0.001);
+PID Tp(150,40000000,0,0.001);
+//PID Tp(4.,40000,0,0.001);
 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);
-//Moter ajust(p24,p13,p14);
-BusOut ajust(p13,p14);
-PwmOut ajstpwm(p24);
 DigitalIn limit1(p11,PullUp);
 DigitalIn limit2(p12,PullUp);
 Timeout ai;
@@ -47,11 +45,12 @@
 void rev()
 {
     air = 2;
-    ai.attach(&zero,1.0);
+    ai.attach(&zero,shoottime);
     out = 0;
 }
 int main()
 {
+    led = 1;
     double tilt = 0,lo = 0,ro = 0;
     int8_t ttilt = 0,tmpread = 0,tmpttilt = 0,ajst = 0;
     char tro = 0,tlo = 0;
@@ -59,6 +58,10 @@
     Tp.setOutputLimits(-0.9,0.9);
     Tp.setMode(1);
     Tp.setBias(0.0);
+    L = 0.5;
+    R = 0.5;
+    wait(3);
+    led= 15;
     while(1) {
         if(conn.getc() == 255) {
             tmpread = conn.getc();
@@ -69,13 +72,13 @@
             if(tmpread^tmpttilt^tro^tlo == conn.getc()) {
                 ttilt = tmpttilt;
                 read = tmpread;
-                ro = (tro-127)/127.0*0.9;
+                ro = (tro-117)/127.0*0.9;
                 lo = (tlo-127)/127.0*0.9;
             }
         }
         if((read>>2)%2 && i == 0) {
             air = 1;
-            ai.attach(&rev,1.0);
+            ai.attach(&rev,shoottime);
             i = 1;
         }
         /*補給制御*/
@@ -93,18 +96,6 @@
             su2 = 0;
             sup2.speed(0);
         }
-        /*輪の調整用ｶｯｺｶﾘ*/
-        if(ajst == 1){
-            ajust = 1;
-            ajstpwm = 0.8;
-        }else if(ajust == 2){
-            ajust = 2;
-            
-            ajstpwm = 0.8;
-        }else{
-            ajust = 0;
-            ajstpwm = 1;
-        }
         /*射角制御*/
         Tp.setSetPoint(ttilt);
         tilt = double(sensort.getPulses());
@@ -117,14 +108,17 @@
         if (abs(ro) < 0.1) {
             ro = 0;
         }
+        lo = lo*abs(lo);
+        ro = ro*abs(ro);
         /*スレーブにreadを送信*/
         slave.putc(read);
         /*各モーターに出力*/
-        L = (lo+1.0)/2.0;
-        R = (ro+1.0)/2.0;;
+        L = (ro+1.0)/2.0;
+        R = 1.0-(lo+1.0)/2.0;
         ot.speed(Tp.compute());
-        pc.printf("%d\n\r",sensort.getPulses());
-        //pc.printf("%f %d\n\r",Tp.compute(),read);
+        //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(1);
     }