動くといいな
Dependencies: Motor_NIT_Nagaoka_College PID Servo QEI SoftPWM mbed HBridge
main.cpp
- Committer:
- WAT34
- Date:
- 2015-10-05
- Revision:
- 16:6b8766c77d29
- Parent:
- 15:a9b36208dc32
- Child:
- 17:c55a8b8a3eb2
- Child:
- 18:7703c9baf008
File content as of revision 16:6b8766c77d29:
#define pi 3.141593 #include "mbed.h" #include "Motor.h" #include "PID.h" #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); Serial pc(USBTX,USBRX); Serial slave(p28,p27); Servo L(p25); Servo R(p26); 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); DigitalIn limit1(p11,PullUp); DigitalIn limit2(p12,PullUp); Timeout ai; char read; int Rs = 0,Ls = 0,d=0,su1 = 0,su2 = 0; int i = 0; void supply() { if(d%2){ sup1.speed(0.9); }else{ sup2.speed(0.9); } d++; } void zero() { air = 0; i = 0; out = 1; supply(); } void rev() { air = 2; 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; Tp.setInputLimits(-45,45); 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(); tmpttilt = conn.getc(); tro = conn.getc(); tlo = conn.getc(); ajst = conn.getc(); if(tmpread^tmpttilt^tro^tlo == conn.getc()) { ttilt = tmpttilt; read = tmpread; 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,shoottime); 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*55/2684.0; Tp.setProcessValue(tilt); /*足の出力が小さい場合はゼロとする*/ if (abs(lo) < 0.1) { lo = 0; } if (abs(ro) < 0.1) { ro = 0; } lo = lo*abs(lo); ro = ro*abs(ro); /*スレーブにreadを送信*/ slave.putc(read); /*各モーターに出力*/ L = (ro+1.0)/2.0; R = 1.0-(lo+1.0)/2.0; ot.speed(Tp.compute()); //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); } }