PS3 version
Dependencies: Motor_NIT_Nagaoka_College PID QEI SoftServo SoftPWM mbed DebounceIn
Fork of NHK2015 by
main.cpp
- Committer:
- WAT34
- Date:
- 2015-10-01
- Revision:
- 15:a9b36208dc32
- Parent:
- 14:5722e243f843
- Child:
- 16:6b8766c77d29
File content as of revision 15:a9b36208dc32:
#define pi 3.141593 #include "mbed.h" #include "Motor.h" #include "PID.h" #include "QEI.h" #include "Servo.h" #define RATE 0.05 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(50,40000,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; 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,1.0); out = 0; } int main() { 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); 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-127)/127.0*0.9; lo = (tlo-127)/127.0*0.9; } } 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); } /*輪の調整用カッコカリ*/ 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()); tilt = tilt*55/2684.0; Tp.setProcessValue(tilt); /*足の出力が小さい場合はゼロとする*/ if (abs(lo) < 0.1) { lo = 0; } if (abs(ro) < 0.1) { ro = 0; } /*スレーブにreadを送信*/ slave.putc(read); /*各モーターに出力*/ L = (lo+1.0)/2.0; R = (ro+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("%d-%d\r\n",tlo,tro); wait_ms(1); } }