PS3 version
Dependencies: Motor_NIT_Nagaoka_College PID QEI SoftServo SoftPWM mbed DebounceIn
Fork of NHK2015 by
main.cpp
- Committer:
- eil4nyqn
- Date:
- 2015-09-07
- Revision:
- 6:21f6a2216fad
- Parent:
- 5:4b462b9cb255
- Child:
- 7:920cbfb28112
File content as of revision 6:21f6a2216fad:
#define pi 3.141593 #include "mbed.h" #include "Motor.h" #include "PID.h" #include "QEI.h" #define RATE 0.05 BusOut air(p13,p14); DigitalOut out(p12); Serial conn(p28,p27); Serial pc(USBTX,USBRX); Motor L(p21,p15,p16); Motor R(p22,p17,p18); PID Tp(50,40000,0,0.001); BusOut led(LED1,LED2,LED3,LED4); Motor ot(p23,p19,p20); QEI sensort(p29,p30,NC,624); Timeout ai; char read; int Rs = 0,Ls = 0; int i = 0; void zero(){ air = 0; i = 0; out = 1; } 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; 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(); 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; } Tp.setSetPoint(ttilt); tilt = double(sensort.getPulses()); tilt = tilt*61/5128.0; Tp.setProcessValue(tilt); if (abs(lo) < 0.1){ lo = 0; } if (abs(ro) < 0.1){ ro = 0; } L.speed(lo); R.speed(ro); ot.speed(Tp.compute()); pc.printf("%d %d\n\r",Ls,read); //pc.printf("%d-%d\r\n",tlo,tro); wait_ms(1); } }