PS3 version
Dependencies: Motor_NIT_Nagaoka_College PID QEI SoftServo SoftPWM mbed DebounceIn
Fork of NHK2015 by
Diff: main.cpp
- Revision:
- 11:427bb7a43d7a
- Parent:
- 7:920cbfb28112
- Child:
- 13:400d640bb447
--- a/main.cpp Thu Sep 10 01:43:12 2015 +0000 +++ b/main.cpp Thu Sep 17 01:07:52 2015 +0000 @@ -3,27 +3,45 @@ #include "Motor.h" #include "PID.h" #include "QEI.h" +#include "Servo.h" #define RATE 0.05 -BusOut air(p13,p14); -DigitalOut out(p12); -Serial conn(p28,p27); +BusOut air(p5,p6); +DigitalOut out(p7); +Serial conn(NC,p27); Serial pc(USBTX,USBRX); -Motor L(p21,p15,p16); -Motor R(p22,p17,p18); +Serial slave(p9,NC); +Servo L(p24); +Servo R(p25); PID Tp(50,40000,0,0.001); BusOut led(LED1,LED2,LED3,LED4); -Motor ot(p23,p19,p20); +Motor ot(p23,p13,p14); QEI sensort(p29,p30,NC,624); +Motor sup1(p21,p15,p17); +Motor sup2(p22,p19,p20); +BusIn limit1(p10); +BusIn limit2(p11); Timeout ai; char read; -int Rs = 0,Ls = 0; +int Rs = 0,Ls = 0,d=0,su1 = 0,su2 = 0; int i = 0; -void zero(){ +void supply() +{ + if(d%2){ + sup1.speed(1); + }else{ + sup2.speed(1); + } + +} +void zero() +{ air = 0; i = 0; out = 1; + supply(); } -void rev(){ +void rev() +{ air = 2; ai.attach(&zero,1.0); out = 0; @@ -43,30 +61,50 @@ tmpttilt = conn.getc(); tro = conn.getc(); tlo = conn.getc(); - if(tmpread^tmpttilt^tro^tlo == conn.getc()){ + if(tmpread^tmpttilt^tro^tlo == conn.getc()) { ttilt = tmpttilt; read = tmpread; ro = (tro-127)/127.0*0.9; lo = (tlo-127)/127.0*0.9; } } - if((read>>2)%2 && i == 0){ + if((read>>2)%2 && i == 0) { air = 1; ai.attach(&rev,1.0); i = 1; } + /*補給制御*/ + if(limit1 == 0) { + su1 = 1; + } + if(limit2 == 0){ + su2 = 1; + } + if(limit1 == 1 && su1 == 1) { + su1 = 0; + sup1.speed(0); + } + if(limit2 == 1 && su2 == 1) { + su2 = 0; + sup2.speed(0); + } + /*射角制御*/ Tp.setSetPoint(ttilt); tilt = double(sensort.getPulses()); tilt = tilt*61/5128.0; Tp.setProcessValue(tilt); - if (abs(lo) < 0.1){ + /*足の出力が小さい場合はゼロとする*/ + if (abs(lo) < 0.1) { lo = 0; } - if (abs(ro) < 0.1){ + if (abs(ro) < 0.1) { ro = 0; } - L.speed(lo); - R.speed(ro); + /*スレーブにreadを送信*/ + slave.putc(read); + /*各モーターに出力*/ + L = (lo+1.0)/2.0; + R = (ro+1.0)/2.0;; ot.speed(Tp.compute()); //pc.printf("%f %d\n\r",Tp.compute(),read); //pc.printf("%d-%d\r\n",tlo,tro);