Diff: main.cpp

Revision:

13:400d640bb447

Parent:

11:427bb7a43d7a

Child:

14:5722e243f843

--- a/main.cpp	Thu Sep 17 09:08:39 2015 +0000
+++ b/main.cpp	Mon Sep 28 06:34:18 2015 +0000
@@ -10,16 +10,20 @@
 Serial conn(NC,p27);
 Serial pc(USBTX,USBRX);
 Serial slave(p9,NC);
-Servo L(p24);
-Servo R(p25);
-PID Tp(50,40000,0,0.001);
+Servo L(p25);
+Servo R(p26);
+//PID Tp(50,40000,0,0.001);
+PID Tp(4.,40000,0,0.001);
 BusOut led(LED1,LED2,LED3,LED4);
-Motor ot(p23,p13,p14);
+Motor ot(p23,p15,p16);
 QEI sensort(p29,p30,NC,624);
-Motor sup1(p21,p15,p17);
-Motor sup2(p22,p19,p20);
-BusIn limit1(p10);
-BusIn limit2(p11);
+Motor sup1(p21,p19,p20);
+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;
 char read;
 int Rs = 0,Ls = 0,d=0,su1 = 0,su2 = 0;
@@ -49,7 +53,7 @@
 int main()
 {
     double tilt = 0,lo = 0,ro = 0;
-    int8_t ttilt = 0,tmpread = 0,tmpttilt = 0;
+    int8_t ttilt = 0,tmpread = 0,tmpttilt = 0,ajst = 0;
     char tro = 0,tlo = 0;
     Tp.setInputLimits(-45,45);
     Tp.setOutputLimits(-0.9,0.9);
@@ -61,6 +65,7 @@
             tmpttilt = conn.getc();
             tro = conn.getc();
             tlo = conn.getc();
+            ajst = conn.getc();
             if(tmpread^tmpttilt^tro^tlo == conn.getc()) {
                 ttilt = tmpttilt;
                 read = tmpread;
@@ -88,6 +93,17 @@
             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());