yuto kawamura
/
George_Slave_BOTHMOVE
right and left move at the same time
Diff: main.cpp
- Revision:
- 1:34371ffd3dc0
- Parent:
- 0:411ab20ce87d
- Child:
- 2:db2bc2ae4d20
diff -r 411ab20ce87d -r 34371ffd3dc0 main.cpp --- a/main.cpp Fri Apr 26 11:34:44 2019 +0000 +++ b/main.cpp Sat Apr 27 05:34:43 2019 +0000 @@ -3,22 +3,25 @@ + ////////////関数 void setup(); void can_receive(); void pid(double,double); void out_ro(double); void out_ri(double); +void reset(); ////////////定数 -int solution=500; -int c_degree=360/500; //solution=500 +int solution=1000; +double c_degree=0.36; //solution=500 double Kp_ro=0.01; double Ki_ro=0; double Kd_ro=0; double Kp_ri=0.01; + double Ki_ri=0; double Kd_ri=0; @@ -39,65 +42,98 @@ ////////////////////////////////////////////// -int main() { - +int main() +{ + setup(); + reset(); while(1) { + can_receive(); pid(target_ro,target_ri); wait(0.01); } } ////////////////////////////////////////////// -void setup(){ +void setup() +{ can.frequency(1000000); motor_ro_f.period_us(100); motor_ro_b.period_us(100); motor_ri_f.period_us(100); motor_ri_b.period_us(100); - - switch1.mode(PullUp); - switch2.mode(PullUp); - + + switch_ro.mode(PullUp); + switch_ri.mode(PullUp); + servo.init(); timer.start(); } +void reset() +{ + while(switch_ro.read()) { + out_ro(0.05); + } + ec_ro.reset(); + out_ro(0); + while(switch_ri.read()) { + out_ri(0.05); + } + + ec_ri.reset(); + out_ri(0); +} -void pid(double target_ro_,double target_ri_){ +void pid(double target_ro_,double target_ri_) +{ posi_ro=(ec_ro.getCount()%solution)*c_degree; + if(posi_ro<0)posi_ro+=360; posi_ri=(ec_ri.getCount()%solution)*c_degree; - + if(posi_ri<0)posi_ri+=360; + double now=timer.read(); double d_time=now-pre_time; - + double deviation_ro=fabs(target_ro_)-posi_ro; - if(target_ro_>0){ - if(deviation_ro<0)deviation_ro=360+deviation_ro; - }else{ - if(deviation_ro>0)deviation_ro=deviation_ro-360; + if(fabs(deviation_ro)<90) { //そのまま + } else if(deviation_ro>270) { + deviation_ro-=360; + } else if(deviation_ro<-270) { + deviation_ro+=360; + } else if(target_ro_>0) { + if(deviation_ro<0)deviation_ro+=360; + } else { + if(deviation_ro>0)deviation_ro-=360; } + double deviation_ri=fabs(target_ri_)-posi_ri; - if(target_ri_>0){ - if(deviation_ri<0)deviation_ri=360+deviation_ri; - }else{ - if(deviation_ri>0)deviation_ri=deviation_ri-360; + if(fabs(deviation_ri)<90) { //そのまま + } else if(deviation_ri>270) { + deviation_ri-=360; + } else if(deviation_ri<-270) { + deviation_ri+=360; + } else if(target_ri_>0) { + if(deviation_ri<0)deviation_ri+=360; + } else { + if(deviation_ri>0)deviation_ri-=360; } - + pile_ro+=deviation_ro; pile_ri+=deviation_ri; - + out_ro(deviation_ro * Kp_ro + (posi_ro - distance_ro_old) / d_time * Kd_ro + pile_ro * Ki_ro * d_time); out_ri(deviation_ri * Kp_ri + (posi_ri - distance_ri_old) / d_time * Kd_ri + pile_ri * Ki_ri * d_time); - + distance_ro_old=deviation_ro; distance_ri_old=deviation_ri; pre_time=now; } -void out_ro(double duty){ - double dutylimit=0.4; +void out_ro(double duty) +{ + double dutylimit=0.1; if(duty > 0) { //入力duty比が正の場合 //if(duty-pre_out_r >accel_max && pre_out_l*duty>0)duty=pre_out_r+accel_max; @@ -122,8 +158,9 @@ } -void out_ri(double duty){ - double dutylimit=0.4; +void out_ri(double duty) +{ + double dutylimit=0.1; if(duty > 0) { //入力duty比が正の場合 //if(duty-pre_out_r >accel_max && pre_out_l*duty>0)duty=pre_out_r+accel_max; @@ -148,18 +185,19 @@ } //////////////////////////////////////////can -void can_receive(){ +void can_receive() +{ CANMessage msg; for(int i=0; i<5; i++) { if(can.read(msg)) { if(msg.id==0) { - target_ro= msg.data[0] | ((msg.data[1]&0b1111)<<8) - 360; - target_ri= msg.data[2] | ((msg.data[1]&0b11110000)<<4) - 360; + target_ro= msg.data[0] + ((msg.data[2]&0b1111)<<8) - 360; + target_ri= msg.data[1] + ((msg.data[2]&0b11110000)<<4) - 360; hand_mode= msg.data[3]; - + break; } - led2=1; + led2=1; } else led2=0; } }