![](/media/cache/group/gazou_0224.jpg.50x50_q85.jpg)
PS3 version
Dependencies: Motor_NIT_Nagaoka_College PID QEI SoftServo SoftPWM mbed DebounceIn
Fork of NHK2015 by
Diff: main.cpp
- Revision:
- 16:6b8766c77d29
- Parent:
- 15:a9b36208dc32
- Child:
- 17:c55a8b8a3eb2
- Child:
- 18:7703c9baf008
diff -r a9b36208dc32 -r 6b8766c77d29 main.cpp --- 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); }