Diff: main.cpp

Revision:

24:7e93db7d1c32

Parent:

23:bfe8c3791f1f

--- a/main.cpp	Mon Oct 26 07:10:02 2015 +0000
+++ b/main.cpp	Fri Oct 30 01:57:54 2015 +0000
@@ -19,7 +19,6 @@
 Servo L(p25);
 Servo R(p26);
 PID Tp(50,4000000,0,0.001);
-//PID Tp(4.,40000,0,0.001);
 BusOut led(p8,p13,p14,p24);
 Motor ot(p23,p20,p19);
 QEI sensort(p29,p30,NC,624);
@@ -68,13 +67,13 @@
 }
 void supply_1()
 {
-    sup1_speed(0.9);
+    sup1_speed(0.4);
     target_count1 +=3;
     supply_seq1 = 1;
 }
 void supply_2()
 {
-    sup2_speed(0.9);
+    sup2_speed(0.4);
     target_count2 +=3;
     supply_seq2 = 1;
 }
@@ -93,7 +92,7 @@
 void count_check()
 {
     if((target_count1 == count1)&&(supply_seq1 == 1)) {
-        sup1_speed(-0.9);
+        sup1_speed(-0.4);
         target_count1 += 2;
         supply_seq1 = 2;
     } else if((target_count1 == count1) && (supply_seq1 == 2)) {
@@ -102,7 +101,7 @@
         mled = 0;
     }
     if((target_count2 == count2)&&(supply_seq2 == 1)) {
-        sup2_speed(-0.9);
+        sup2_speed(-0.4);
         target_count2 += 2;
         supply_seq2 = 2;
     } else if((target_count2 == count2) && (supply_seq2 == 2)) {
@@ -137,6 +136,8 @@
 }
 int main()
 {
+    L = 0.5;
+    R = 0.5;
     limit1.mode(PullUp);
     limit2.mode(PullUp);
     led = 1;
@@ -148,12 +149,7 @@
     Tp.setOutputLimits(-0.9,0.9);
     Tp.setMode(1);
     Tp.setBias(0.0);
-    Tp.setProcessValue(sensort.getPulses()*49.0/1745.0);
-    Tp.setSetPoint(0);
-    ot.speed(0);
-    L = 0.5;
-    R = 0.5;
-    wait(5);
+    wait(3);
     led= 15;
     conn.baud(38400);
     while(1) {
@@ -200,19 +196,11 @@
         count_check();
         /*射角制御*/
         Tp.setSetPoint((ttilt-60)/2.0);
-        tilt = double(sensort.getPulses());
+        tilt = double(-sensort.getPulses());
         tilt = tilt*49.0/1745.0;
         Tp.setProcessValue(tilt);
-        ro *= abs(ro)*0.5;
-        lo *= abs(lo)*0.5;
         /*足の出力が小さい場合はゼロとする*/
-        if (abs(lo) < 0.1) {
-            lo = 0;
-        }
-        if (abs(ro) < 0.1) {
-            ro = 0;
-        }
-        if(timer1.read_ms() > DEADTIME) {
+        /*if(timer1.read_ms() > DEADTIME) {
             dead1 = 0;
             timer1.stop();
             timer1.reset();
@@ -239,13 +227,17 @@
         } else {
             R = (ro+1.0)/2.0;
             Rl = (ro+1.0)/2.0;
-        }
+        }*/
+        R = (ro+1.0)/2.0;
+        Rl = (ro+1.0)/2.0;
+        L = (lo+1.0)/2.0;
+        Ll = (lo+1.0)/2.0;
         /*スレーブにreadを送信*/
         slave.putc(read);
         /*各モーターに出力*/
-        pc.printf("%d\n\r",narasup);
+        //pc.printf("%d\n\r",narasup);
         ot.speed(Tp.compute());
-        //pc.printf("%d\n\r",sensort.getPulses());
+        pc.printf("%f\n\r",R);
         //pc.printf("%f \n\r",Tp.compute());
     }
 }