yuto kawamura
/
George_Slave_BOTHMOVE
right and left move at the same time
Diff: main.cpp
- Revision:
- 0:411ab20ce87d
- Child:
- 1:34371ffd3dc0
diff -r 000000000000 -r 411ab20ce87d main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Apr 26 11:34:44 2019 +0000 @@ -0,0 +1,165 @@ +#include "mbed.h" +#include "pin.h" + + + +////////////関数 +void setup(); +void can_receive(); +void pid(double,double); +void out_ro(double); +void out_ri(double); + + +////////////定数 +int solution=500; +int c_degree=360/500; //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; + + + + +////////////変数 +double target_ro=0,target_ri=0; +bool hand_mode=0; +double distance_ro_old=0,distance_ri_old=0,pile_ro=0,pile_ri=0; +double posi_ro=0,posi_ri=0; +double pre_time=0; + + +Timer timer; + + + + +////////////////////////////////////////////// +int main() { + + setup(); + while(1) { + pid(target_ro,target_ri); + wait(0.01); + } +} + +////////////////////////////////////////////// +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); + + servo.init(); + timer.start(); +} + + +void pid(double target_ro_,double target_ri_){ + posi_ro=(ec_ro.getCount()%solution)*c_degree; + posi_ri=(ec_ri.getCount()%solution)*c_degree; + + 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; + } + 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; + } + + 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; + + if(duty > 0) { //入力duty比が正の場合 + //if(duty-pre_out_r >accel_max && pre_out_l*duty>0)duty=pre_out_r+accel_max; + if( fabs( duty ) < dutylimit ) { //制限値内 + motor_ro_f = fabs(duty); + motor_ro_b = 0; + } else { //制限値超 + motor_ro_f = dutylimit; + motor_ro_b = 0; + } + } else {//入力duty比が負の場合 + //if(pre_out_r-duty >accel_max && pre_out_l*duty>0)duty=pre_out_r-accel_max; + if( fabs(duty) < dutylimit) { //制限値内 + motor_ro_f = 0; + motor_ro_b = fabs(duty); + } else { //制限値超 + motor_ro_f = 0; + motor_ro_b = dutylimit; + } + } + //pre_out_r=duty; +} + + +void out_ri(double duty){ + double dutylimit=0.4; + + if(duty > 0) { //入力duty比が正の場合 + //if(duty-pre_out_r >accel_max && pre_out_l*duty>0)duty=pre_out_r+accel_max; + if( fabs( duty ) < dutylimit ) { //制限値内 + motor_ri_f = fabs(duty); + motor_ri_b = 0; + } else { //制限値超 + motor_ri_f = dutylimit; + motor_ri_b = 0; + } + } else {//入力duty比が負の場合 + //if(pre_out_r-duty >accel_max && pre_out_l*duty>0)duty=pre_out_r-accel_max; + if( fabs(duty) < dutylimit) { //制限値内 + motor_ri_f = 0; + motor_ri_b = fabs(duty); + } else { //制限値超 + motor_ri_f = 0; + motor_ri_b = dutylimit; + } + } + //pre_out_r=duty; +} + +//////////////////////////////////////////can +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; + hand_mode= msg.data[3]; + + break; + } + led2=1; + } else led2=0; + } +}