![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
sls
main.cpp
- Committer:
- yuto17320508
- Date:
- 2019-04-27
- Revision:
- 3:7a608fbd3bcd
- Parent:
- 2:55c616d2e0fe
- Child:
- 4:96f38805f055
File content as of revision 3:7a608fbd3bcd:
#include "mbed.h" #include "pin.h" #include "microinfinity.h" ////////////関数 void setup(); void can_send(); void pid(double,double); void out_lo(double); void out_li(double); void reset(); ////////////定数 int resolution_lo=1000; int resolution_li=600; double c_degree_lo=0.36; //solution=500 double c_degree_li=360.0/600.0; double Kp_lo=0.01; double Ki_lo=0; double Kd_lo=0; double Kp_li=0.01; double Ki_li=0; double Kd_li=0; ////////////変数 double target_ro=0,target_ri=0; double target_lo=0,target_li=0; bool hand_mode=0; double distance_lo_old=0,distance_li_old=0,pile_lo=0,pile_li=0; double posi_lo=0,posi_li=0; double pre_time=0; Timer timer; ///////////////////////////////////////////// int main() { setup(); reset(); while(1){ target_lo = 180; target_li = 360; target_ri = 360; target_ro = 180; for(int i=0;i<100;++i){ can_send(); pid(target_lo,target_li); //printf("%f,%f\r\n",posi_li,posi_lo); wait(0.01); } target_lo = 360; target_li = 180; target_ri = 180; target_ro = 360; for(int i=0;i<100;++i){ can_send(); pid(target_lo,target_li); //printf("%f,%f\r\n",posi_li,posi_lo); wait(0.01); } } } void setup() { can1.frequency(1000000); motor_lo_f.period_us(100); motor_lo_b.period_us(100); motor_li_f.period_us(100); motor_li_b.period_us(100); hand.mode(PullUp); switch_lo.mode(PullUp); switch_li.mode(PullUp); switch4.mode(PullUp); device.baud(115200); device.format(8,Serial::None,1); device.attach(dev_rx, Serial::RxIrq); wait(0.05); theta0=degree0; check_gyro(); timer.start(); } //////////////////////////////////////can void can_send() { char data[4]= {0}; int target_ro_send=target_ro+360; int target_ri_send=target_ri+360; data[0]=target_ro_send & 0b11111111; data[1]=target_ri_send & 0b11111111; data[2]=(target_ro_send>>8) | ((target_ri_send>>4) & 0b11110000); data[3]=hand_mode; if(can1.write(CANMessage(0,data,4)))led4=1; else led4=0; } void reset() { while(switch_lo.read()) { out_lo(0.05); } ec_lo.reset(); out_lo(0); while(switch_li.read()) { out_li(0.05); } ec_li.reset(); out_li(0); } void out_lo(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; if( fabs( duty ) < dutylimit ) { //制限値内 motor_lo_f = fabs(duty); motor_lo_b = 0; } else { //制限値超 motor_lo_f = dutylimit; motor_lo_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_lo_f = 0; motor_lo_b = fabs(duty); } else { //制限値超 motor_lo_f = 0; motor_lo_b = dutylimit; } } //pre_out_r=duty; } void out_li(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; if( fabs( duty ) < dutylimit ) { //制限値内 motor_li_f = fabs(duty); motor_li_b = 0; } else { //制限値超 motor_li_f = dutylimit; motor_li_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_li_f = 0; motor_li_b = fabs(duty); } else { //制限値超 motor_li_f = 0; motor_li_b = dutylimit; } } //pre_out_r=duty; } void pid(double target_lo_,double target_li_) { posi_lo=(ec_lo.getCount()%resolution_lo)*c_degree_lo; if(posi_lo<0)posi_lo+=360; posi_li=(ec_li.getCount()%resolution_li)*c_degree_li; if(posi_li<0)posi_li+=360; double now=timer.read(); double d_time=now-pre_time; double deviation_lo=fabs(target_lo_)-posi_lo; if(fabs(deviation_lo)<90) { //そのまま } else if(deviation_lo>270) { deviation_lo-=360; } else if(deviation_lo<-270) { deviation_lo+=360; } else if(target_lo_>0) { if(deviation_lo<0)deviation_lo+=360; } else { if(deviation_lo>0)deviation_lo-=360; } double deviation_li=fabs(target_li_)-posi_li; if(fabs(deviation_li)<90) { //そのまま } else if(deviation_li>270) { deviation_li-=360; } else if(deviation_li<-270) { deviation_li+=360; } else if(target_li_>0) { if(deviation_li<0)deviation_li+=360; } else { if(deviation_li>0)deviation_li-=360; } pile_lo+=deviation_lo; pile_li+=deviation_li; out_lo(deviation_lo * Kp_lo + (posi_lo - distance_lo_old) / d_time * Kd_lo + pile_lo * Ki_lo * d_time); out_li(deviation_li * Kp_li + (posi_li - distance_li_old) / d_time * Kd_li + pile_li * Ki_li * d_time); distance_lo_old=deviation_lo; distance_li_old=deviation_li; pre_time=now; }