PS3 version
Dependencies: Motor_NIT_Nagaoka_College PID QEI SoftServo SoftPWM mbed DebounceIn
Fork of NHK2015 by
Diff: main.cpp
- Revision:
- 3:d2c733b52600
- Parent:
- 2:74c543a0a671
- Child:
- 4:646562d80dc2
--- a/main.cpp Thu Aug 06 14:24:04 2015 +0000 +++ b/main.cpp Wed Sep 02 00:19:34 2015 +0000 @@ -1,60 +1,106 @@ -#define pi 3.141592 +#define pi 3.141593 #include "mbed.h" -#include "rtos.h" -#include "QEI.h" #include "Motor.h" -#include "HMC6352.h" -BusOut air(p15,p16); -Serial conn(p28,p27); +#include "PID.h" +#include "QEI.h" +#define RATE 0.01 +BusOut air(p13,p14); +DigitalOut out(p12); +Serial conn(p9,p10); Serial pc(USBTX,USBRX); -BusOut mypaul(p17,p18); -BusIn sw(p24,p25); -Motor od(p21,p19,p20); -Motor ot(p22,p12,p11); +Motor L(p22,p15,p16); +Motor R(p23,p17,p18); +PID Lp(1.0,0,0,RATE); +PID Rp(1.0,0,0,RATE); +PID Tp(1.0,0,0,RATE); +BusOut led(LED1,LED2,LED3,LED4); +Motor ot(p21,p19,p20); QEI sensort(p29,p30,NC,624); -HMC6352 sensord(p9,p10); +QEI ls(p27,p28,NC,624); +QEI rs(p25,p26,NC,624); +Timeout ai; +Ticker getsp; char read; +int Rs = 0,Ls = 0; +int i = 0; +void getspeed() +{ + Rs =rs.getPulses(); + Ls =ls.getPulses(); + rs.reset(); + ls.reset(); +} +void zero(){ + air = 0; + i = 0; + out = 1; +} +void rev(){ + air = 2; + ai.attach(&zero,1.0); + out = 0; +} int main() { - //sensord.setOpMode(HMC6352_CONTINUOUS,1,20); - double direct = 0,tilt = 0,cdirect = 0,adirect = 0,tdirect = 0,ttilt =0,itilt=0; - //tdirect = sensord.sample()/10.0; - sw.mode(PullUp); + double tilt = 0; + int8_t ttilt = 0; + Lp.setInputLimits(-3000,3000); + Rp.setInputLimits(-3000,3000); + Tp.setInputLimits(-10000,10000); + Lp.setOutputLimits(-0.9,0.9); + Rp.setOutputLimits(-0.9,0.9); + Tp.setOutputLimits(-0.9,0.9); + Lp.setMode(1); + Rp.setMode(1); + Tp.setMode(1); + Lp.setBias(0); + Rp.setBias(0); + Tp.setBias(0); + getsp.attach(&getspeed,0.1); while(1) { - if(conn.readable()) { + if(conn.getc() == 255) { read = conn.getc(); + ttilt = conn.getc(); } - air = read%4; - mypaul = (read>>2)%4; - if((read>>4)%2 || 0/*sw == 2*/) { - //tdirect++; - od.speed(0.3); - } else if((read>>5)%2 ||0 /*sw == 1*/) { - //tdirect--; - od.speed(-0.3); - } else { - od.speed(0); + if((read>>2)%2 && i == 0){ + air = 1; + ai.attach(&rev,1.0); + i = 1; + led = 1; } - if((read>>6)%2|| sw == 2 ) { - //ttilt+=pi/180.0*0.01; - ot.speed(0.3); - } else if((read>>7)%2 || sw == 1) { - //ttilt-=pi/180.0*0.01; - ot.speed(-0.3); - } else { - ot.speed(0); + if((read>>3)%2){ + Lp.setSetPoint(100); + Rp.setSetPoint(100); + }else + if((read>>4)%2){ + Lp.setSetPoint(-100); + Rp.setSetPoint(-100); + led = 2; + }else + if((read>>5)%2){ + Lp.setSetPoint(-100); + Rp.setSetPoint(100); + led = 4; + }else + if((read>>6)%2){ + Lp.setSetPoint(100); + Rp.setSetPoint(-100); + led = 8; + }else{ + Lp.setSetPoint(0); + Rp.setSetPoint(0); + led = 0; } - if(tdirect >= 180 ||tdirect <= -180) { - //tdirect *= -1; - } - //cdirect = sensord.sample()/10.0; - direct = cdirect+540-tdirect; - direct = int(direct+0.5)%360; - direct -= 180; - //tilt = sensort.getPulses()/360.0/25.0*pi; - pc.printf("%d\n\r",read); - //od.speed(cdirect); - //ot.speed((ttilt-tilt)*1.0+itilt*0.01); - itilt += ttilt-tilt; + Tp.setSetPoint(ttilt); + tilt = sensort.getPulses(); + //tilt = tilt*24.0/500.0*pi/360.0; + conn.putc(int8_t(tilt)); + Lp.setProcessValue(Ls); + Rp.setProcessValue(Rs); + Tp.setProcessValue(tilt); + L.speed(Lp.compute()); + R.speed(Rp.compute()); + ot.speed(Tp.compute()); + pc.printf("%f\n\r",Lp.compute()); } }