PS3 version
Dependencies: Motor_NIT_Nagaoka_College PID QEI SoftServo SoftPWM mbed DebounceIn
Fork of NHK2015 by
main.cpp
- Committer:
- WAT34
- Date:
- 2015-10-26
- Revision:
- 23:bfe8c3791f1f
- Parent:
- 22:a834fd13fcfd
- Child:
- 24:7e93db7d1c32
File content as of revision 23:bfe8c3791f1f:
#define pi 3.141593 #include "mbed.h" #include "Motor.h" #include "PID.h" #include "QEI.h" #include "Servo.h" #include "DebounceIn.h" #define RATE 0.05 #define shoottime 0.5 #define DEADTIME 500 BusOut air(p5,p6); DigitalOut out(p7); Serial conn(p9,p10); Serial pc(USBTX,USBRX); Serial slave(p28,p27); PwmOut Ll(LED1); PwmOut Rl(LED2); BusOut mled(LED3,LED4); Servo L(p25); Servo R(p26); PID Tp(50,4000000,0,0.001); //PID Tp(4.,40000,0,0.001); BusOut led(p8,p13,p14,p24); Motor ot(p23,p20,p19); QEI sensort(p29,p30,NC,624); //Motor sup1(p21,p15,p16); //Motor sup2(p22,p17,p18); SoftPWM sup1p(p21); BusOut sup1d(p15,p16); SoftPWM sup2p(p22); BusOut sup2d(p18,p17); DebounceIn limit1(p11); DebounceIn limit2(p12); Timeout ai; Timer timer1; Timer timer2; char read; int Rs = 0,Ls = 0,d=0,su1 = 0,su2 = 0; int i = 0; int old_limit1 = 0,old_limit2 = 0,new_limit1 = 0,new_limit2 = 0,count1 = 0,count2 = 0,target_count1 = 0,target_count2 = 0,supply_seq1 = 0,supply_seq2 = 0; void sup1_speed(float speed) { if(speed>0) { sup1d = 1; } else if(speed < 0) { sup1d =2; } sup1p = fabs(speed); } void sup2_speed(float speed) { if(speed>0) { sup2d = 1; } else if(speed < 0) { sup2d =2; } sup2p = fabs(speed); } void sup1_brake() { sup1d = 0; sup1p = 1; } void sup2_brake() { sup2d = 0; sup2p = 1; } void supply_1() { sup1_speed(0.9); target_count1 +=3; supply_seq1 = 1; } void supply_2() { sup2_speed(0.9); target_count2 +=3; supply_seq2 = 1; } void supply() { if (supply_seq1 ==0 && supply_seq2 == 0) { if(d%2) { supply_1(); } else { supply_2(); } d++; } } void count_check() { if((target_count1 == count1)&&(supply_seq1 == 1)) { sup1_speed(-0.9); target_count1 += 2; supply_seq1 = 2; } else if((target_count1 == count1) && (supply_seq1 == 2)) { sup1_brake(); supply_seq1 = 0; mled = 0; } if((target_count2 == count2)&&(supply_seq2 == 1)) { sup2_speed(-0.9); target_count2 += 2; supply_seq2 = 2; } else if((target_count2 == count2) && (supply_seq2 == 2)) { sup2_brake(); supply_seq2 = 0; mled = 0; } } void limit1_count() { new_limit1 = limit1; if ((new_limit1==0) && (old_limit1==1)) count1++; old_limit1 = new_limit1; } void limit2_count() { new_limit2 = limit2; if ((new_limit2==0) && (old_limit2==1)) count2++; old_limit2 = new_limit2; } void zero() { air = 0; i = 0; out = 1; } void rev() { air = 2; ai.attach(&zero,shoottime); out = 0; } int main() { limit1.mode(PullUp); limit2.mode(PullUp); led = 1; int dead1 = 0,dead2 = 0; double tilt = 0,lo = 0,ro = 0; int8_t ttilt = 60,tmpread = 0,tmpttilt = 0,ajst = 0; char tro = 0,tlo = 0,narasup = 60; Tp.setInputLimits(-45,45); Tp.setOutputLimits(-0.9,0.9); Tp.setMode(1); Tp.setBias(0.0); Tp.setProcessValue(sensort.getPulses()*49.0/1745.0); Tp.setSetPoint(0); ot.speed(0); L = 0.5; R = 0.5; wait(5); led= 15; conn.baud(38400); while(1) { //ps3コントローラーシリアル受信 if(conn.getc() == 255) { read = conn.getc(); ttilt = conn.getc(); tro =conn.getc(); tlo = conn.getc(); if (tro == 255){ ro =ro; }else{ ro = (tro-127)/127.0*0.9; } if(tlo ==255){ lo = lo; }else { lo = (tlo-127)/127.0*0.9; } } //nara シリアル受信 if(slave.readable()) { narasup = slave.getc(); if((narasup-60) == 1 && supply_seq1 == 0) { mled = 1; supply_1(); } else if((narasup-60) == 2 && supply_seq2 == 0) { supply_2(); mled =2; } } if (read > 4){ //通信不良対策 read = 0; } led = read; if(read==2 && i == 0) { air = 1; ai.attach(&rev,shoottime); i = 1; supply(); } limit1_count(); limit2_count(); count_check(); /*射角制御*/ Tp.setSetPoint((ttilt-60)/2.0); tilt = double(sensort.getPulses()); tilt = tilt*49.0/1745.0; Tp.setProcessValue(tilt); ro *= abs(ro)*0.5; lo *= abs(lo)*0.5; /*足の出力が小さい場合はゼロとする*/ if (abs(lo) < 0.1) { lo = 0; } if (abs(ro) < 0.1) { ro = 0; } if(timer1.read_ms() > DEADTIME) { dead1 = 0; timer1.stop(); timer1.reset(); } if(timer2.read_ms() > DEADTIME) { dead2 = 0; timer2.stop(); timer2.reset(); } if ((L >0.5 && (lo+1.0)/2.0<=0.5) || (L <0.5 && (lo+1.0)/2.0>=0.5) || dead1 ==1) { L = 0.5; Ll = 0.5; dead1 =1; timer1.start(); } else { L = (lo+1.0)/2.0; Ll = (lo+1.0)/2.0; } if ( (R >0.5 && (ro+1.0)/2.0<=0.5) || (R <0.5 && (ro+1.0)/2.0>=0.5) || dead2 ==1) { R = 0.5; Rl = 0.5; dead2 = 1; timer2.start(); } else { R = (ro+1.0)/2.0; Rl = (ro+1.0)/2.0; } /*スレーブにreadを送信*/ slave.putc(read); /*各モーターに出力*/ pc.printf("%d\n\r",narasup); ot.speed(Tp.compute()); //pc.printf("%d\n\r",sensort.getPulses()); //pc.printf("%f \n\r",Tp.compute()); } }