ロボステ6期
/
NHK2020-arm-sub
arm
Revision 0:6e2abd0956f1, committed 2019-12-14
- Comitter:
- yuki0701
- Date:
- Sat Dec 14 12:42:13 2019 +0000
- Commit message:
- arm;
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SpeedController/Encoder.lib Sat Dec 14 12:42:13 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/2019-11/code/Encoder_/#9a21d4def4c6
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SpeedController/SpeedController.cpp Sat Dec 14 12:42:13 2019 +0000 @@ -0,0 +1,99 @@ +#include "mbed.h" +#include "EC.h" +#include "SpeedController.h" + +SpeedControl::SpeedControl(PinName pwm_F,PinName pwm_B,int us,Ec &ec) : + Kp_(0),Kd_(0),pre_diff(0),ptsc_(0),Cf_(1/45),Cb_(1/45),Df_(0),Db_(0),initial_Df_(0),initial_Db_(0),duty_limit_(0.95),pwm_F_(pwm_F),pwm_B_(pwm_B) +{ + ec_=&ec; + pwm_F_.period_us(us); + pwm_B_.period_us(us); +} +void SpeedControl::period(double s) +{ + pwm_F_.period(s); + pwm_B_.period(s); +} +void SpeedControl::period_ms(int ms) +{ + pwm_F_.period_ms(ms); + pwm_B_.period_ms(ms); +} +void SpeedControl::period_us(int us) +{ + pwm_F_.period_us(us); + pwm_B_.period_us(us); +} + + +void SpeedControl::Sc(double target_omega) //スカンジウムじゃないよ +{ + double t=ec_->timer_.read(); + ec_->calOmega(); + double omega=ec_->getOmega(); + double diff=target_omega-omega; + double pid=Kp_*diff+Kd_*(diff-pre_diff)/(t-ptsc_); + double duty; + if(target_omega>0) { + Df_+=Cf_*pid; + duty=Cf_*target_omega+Df_; + } else if(target_omega<0) { + Db_+=Cb_*pid; + duty=Cb_*target_omega+Db_; + } else duty=0; + turn(duty); + pre_diff=diff; + ptsc_=t; +} + +void SpeedControl::turn(double duty) +{ + if(duty>duty_limit_)duty=duty_limit_; + else if(duty<-duty_limit_)duty=-duty_limit_; + + if(duty>=0) { + pwm_F_=duty; + pwm_B_=0; + } else if(duty<0) { + pwm_F_=0; + pwm_B_=-duty; + } +} + +void SpeedControl::setPDparam(double kp,double kd) +{ + Kp_=kp; + Kd_=kd; +} + + +void SpeedControl::setEquation(double cf,double df,double cb,double db) +{ + Cf_=cf; + if(cb>0)Cb_=cb; + else Cb_=-cb; + initial_Df_=df; + initial_Db_=-db; + Df_=initial_Df_; + Db_=initial_Db_; +} +void SpeedControl::setDutyLimit(double duty_limit) +{ + if(0<=duty_limit && duty_limit<0.95f)duty_limit_=duty_limit; + else duty_limit_=0.95f; +} + +void SpeedControl::reset() +{ + ec_->reset(); + pre_diff=0; + ptsc_=0; + Df_=initial_Df_; + Db_=initial_Db_; +} + +void SpeedControl::stop() +{ + pwm_F_=0; + pwm_B_=0; +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SpeedController/SpeedController.h Sat Dec 14 12:42:13 2019 +0000 @@ -0,0 +1,184 @@ +#ifndef _INCLUDE_SPEEDCONTROLLER_H_ +#define _INCLUDE_SPEEDCONTROLLER_H_ + +/** +* @brief モーターの速度制御用プログラム +* @details モーターのdutyと角速度がほぼ比例することを利用して出力を決定し、PID制御で補正する +**/ +class SpeedControl +{ +private: + double Kp_,Kd_; + double pre_diff; + double ptsc_; + double Cf_; + double Cb_; + double Df_; + double Db_; + double initial_Df_; + double initial_Db_; + double duty_limit_; + Ec *ec_; + +public: + PwmOut pwm_F_; + PwmOut pwm_B_; + + /** constractorの定義 + * @param pwm_F モーターを正転させるpwmピン名 + * @param pwm_B モーターを後転させるpwmピン名 + * @param us モーターのpwm周期[us] + * @param ec Encoder classのポインタ + * モーターを正転させるとエンコーダのcountが正のほうに加算されるようにエンコーダとモーターを設置する + */ + SpeedControl(PinName pwm_F,PinName pwm_B,int us,Ec &ec); + + + + /**モーターのpwm周期の設定用関数 + * @param s モーターのpwm周期[s] + */ + void period(double s); + /**モーターのpwm周期の設定用関数 + * @param ms モーターのpwm周期[ms] + */ + void period_ms(int ms); + /**モーターのpwm周期の設定用関数 + * @param us モーターのpwm周期[us] + * + * @senction SAMPLE + * @code + * Ec4multi EC(p11,p12,500); + * SpeedControl motor(p21,p22,50,EC); + * int main(){ + * motor.period_us(50); + * while(1){ + * @endcode + */ + void period_us(int us); + /** PIDパラメータ設定関数 + * + * 引数はそれぞれp,dパラメータ + * + * パラメーター値は実験的に頑張って求めるしかない + * @param kp p制御パラメータ + * @param kd d制御パラメータ + * + * @senction SAMPLE + * @code + * Ec4multi EC(p11,p12,500); + * SpeedControl motor(p21,p22,50,EC); + * int main(){ + * motor.setPDparam(0.1,0.001); + * while(1){ + * @endcode + */ + void setPDparam(double kp,double kd); + + /** 角速度・duty比の関係式の設定関数 + * 横軸を角速度[rad/s], 縦軸をduty比としたときの傾きCと切片Dを正転と逆転について設定する + * @param cf 角速度[rad/s]とduty比の関係式の傾き(正転時) + * @param df 角速度[rad/s]とduty比の関係式の切片(正転時) + * @param cb 角速度[rad/s]とduty比の関係式の傾き(逆転時) + * @param db 角速度[rad/s]とduty比の関係式の切片(逆転時) + * + * @senction SAMPLE + * @code + * Ec4multi EC(p11,p12,500); + * SpeedControl motor(p21,p22,50,EC); + * int main(){ + * motor.setEquation(0.02,0.0001,-0.02,0.0001); + * while(1){ + * @endcode + */ + void setEquation(double cf,double df,double cb,double db); + /** 出力duty比の上限を決める関数 + * 0 ~ 0.95の範囲内で設定する + * @param duty_limit 出力duty比の上限 + * + * @senction SAMPLE + * @code + * Ec4multi EC(p11,p12,500); + * SpeedControl motor(p21,p22,50,EC); + * int main(){ + * motor.setDutyLimit(0.5); //duty比の上限を0.5に設定 + * while(1){ + * @endcode + */ + void setDutyLimit(double duty_limit); + /** 速度制御関数、引数は目標速度 + * + * この目標角加速度になるようにモーターを回転させることができる + * 負の数を代入すれば逆回転することができる + * + * @param target_omega 目標角速度 + * + * @section SAMPLE + * @code + * //プログラム例 + * #include "mbed.h" + * #include "EC.h" //Encoderライブラリをインクルード + * #include "SpeedController.h" //SpeedControlライブラリをインクルード + * #define RESOLUTION 500 + * Ec4multi EC(p11,p12,RESOLUTION); + * SpeedControl motor(p21,p22,50,EC); + * int main(){ + * motor.setEquation(0.0062,0.0045,-0.0061,0.0091); //求めたC,Dの値を設定 + * motor.setPDparam(0.1,0.0); //PIDの係数を設定 + * int kai=0; + * for(int i=0;i<100000000;i++){ + * motor.Sc(10); //10[rad/s]でモーターを回転 + * kai++; + * if(kai>1000){ + * printf("omega=%f\r\n",EC.getOmega()); + * kai=0; + * } + * } + * motor.stop() + * while(1); + * } + * @endcode + * + * @section CAUTION(printfの負荷について) + * printfはプログラム的に大きな負担がかかり、かなり時間がかかってしまうものである。 + * + * なのでループごとにいちいちprintfをさせると、速度の収束にかなり時間がかかってしまう。 + * + * (どんなプログラムにも言えるが、)そのためこのようなprintfの頻度を少なくさせるような工夫が必要になる。 + */ + void Sc(double target_omega); + /** モーター停止用関数 + * dutyを0にする + */ + void stop(); + /** duty(-1 ~ 1)比でモーターを回転させる関数 + * 負の値を入れたら逆回転する + * @param duty 出力duty比 + * @section SAMPLE + * @code + * #include "mbed.h" + * #include "EC.h" //Encoderライブラリをインクルード + * #include "SpeedController.h" //SpeedControlライブラリをインクルード + * #define RESOLUTION 500 + * Ec4multi EC(p11,p12,RESOLUTION); + * SpeedControl motor(p21,p22,50,EC); + * int main(){ + * while(1){ + * motor.turn(0.5); //duty0.5で正転 + * wait(1); + * motor.turn(0); //停止 + * wait(1); + * motor.turn(-0.5); //duty0.5で逆転 + * wait(1); + * motor.turn(0); //停止 + * wait(1); + * } + * } + * @endcode + */ + void turn(double duty); + ///Encoderや速度制御の値をresetする関数 + void reset(); +}; + +#endif \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Sat Dec 14 12:42:13 2019 +0000 @@ -0,0 +1,250 @@ +#include "mbed.h" +#include "EC.h" //Encoderライブラリをインクルード +#include "SpeedController.h" +#define RESOLUTION 2048//分解能 + +//#define NHK2020_TEST_MODE +#define NHK2020_MAIN_MODE + +CAN can1(p30,p29); + +Ticker can_ticker; //can用ticker + +Ec4multi EC_backdrop(p15,p16,RESOLUTION); +SpeedControl backdrop(p21,p22,50,EC_backdrop); +Serial pc(USBTX,USBRX); +DigitalOut snatch(p8); +DigitalOut pass1(p27); +DigitalOut pass2(p28); +//Ticker motor_tick; //角速度計算用ticker + +char t2[1]= {0}; //動作番号(送信する値(char型)) +int t2_r=0, T2=0; //動作番号(受け取った値、送信する値(int型)) + +double a=0;//now +double b=0;//target +double turn=0; +int k = 0; +void tsukami() +{ + b=1.3; +} +void put() +{ + b=-0.7; +} +void top() +{ + b=0; +} +void move() +{ + double old_turn=turn; + a=EC_backdrop.getRad(); + if(a-b>=0.1) { + turn=0.1; + //pc.printf("F"); + + } else if (a-b>=0.05) { + turn=10*(a-b)*(a-b); + //pc.printf("f"); + } else if (b-a>=0.1) { + turn=-0.1; + //pc.printf("B"); + } else if (b-a>=0.05) { + turn=-10*(a-b)*(a-b); + //pc.printf("b"); + } else { + backdrop.stop(); + backdrop.turn(0); + turn=0; + //break; + //pc.printf("s"); + } + if(turn*old_turn<0)turn=0; + backdrop.turn(turn); +} + +int q = 0; + +void can_read() +{ + + + CANMessage msg; + + if(can1.read(msg)) { + + if(msg.id == 1) { + t2_r = msg.data[0]; + //printf("arm T2 = %d t2_r = %d\n\r",T2,t2_r); + } else { + if(q > 100) { + //printf("id1 fale\n\r"); + q = 0; + } + q++; + } + + } else { + if(k > 100) { + //printf("arm fale\n\r"); + k = 0; + } + k++; + } + + t2[0] = T2; + + if(can1.write(CANMessage(2,t2,1))) { + } + + if(t2_r > T2) { + T2 = t2_r; + } + + +} + + +int main() +{ + move(); + + pc.printf("setting please"); + while(1) { + + + + +#ifdef NHK2020_TEST_MODE + + pc.printf("%lf",EC_backdrop.getRad()); + if(pc.readable()) { + char sel=pc.getc(); + if(sel=='z') { + pc.printf("z\r\n"); + tsukami(); + } else if(sel=='x') { + pc.printf("x\r\n"); + put(); + } else if(sel=='c') { + pc.printf("c\r\n"); + top(); + } + /* if(sel=='q') { + printf("\r\n"); + if(denjiben==0)denjiben=1; + else denjiben=0; + }*/ + if(sel=='1') { + snatch=0; + printf("snatch_off\r\n"); + } + if(sel=='2') { + snatch=1; + printf("snatch_on\r\n"); + } + if(sel=='3') { + pass1=0; + printf("pass1_off\r\n"); + } + if(sel=='4') { + pass1=1; + printf("pass1_on\r\n"); + } + if(sel=='5') { + pass2=0; + printf("pass2_off\r\n"); + } + if(sel=='6') { + pass2=1; + printf("pass2_on\r\n"); + } + // if(sel=='a') { + // //pc.printf("x\r\n"); + // //put(); + // } + } + } +#endif +#ifdef NHK2020_MAIN_MODE + can1.frequency(1000000); + can_ticker.attach(&can_read,0.01); + printf("wait_mode\r\n"); + while(1) { + char sel=pc.getc(); + if(sel=='1') { + snatch=0; + printf("snatch_off\r\n"); + } + if(sel=='2') { + snatch=1; + printf("snatch_on\r\n"); + } + if(sel=='3') { + pass1=0; + printf("pass1_off\r\n"); + } + if(sel=='4') { + pass1=1; + printf("pass1_on\r\n"); + } + if(sel=='5') { + pass2=0; + printf("pass2_off\r\n"); + } + if(sel=='6') { + pass2=1; + printf("pass2_on\r\n"); + } + if(sel=='q') { + printf("end_wait_mode\r\n"); + break; + + } + } + + if(T2 == 0) { //スタート位置からボール前まで移動(アーム待機) + T2=1; + while(1) { + //printf("a-T = 0 T2 = %d t2_r = %d\n\r",T2,t2_r); + wait(0.1); + if(T2 == 1) { + break; + } + } + } + + if(T2 == 1) { //ボール掴んで投げる + //printf("a-T = 1 T2 = %d t2_r = %d\n\r",T2,t2_r); + tsukami(); + move(); + wait(3); + snatch=0; + wait(1); + put(); + wait(3); + snatch=1; + wait(1); + top(); + wait(5); + pass1=0; + wait(5); + //wait(5); + + T2++; + } + if(T2 == 2) { //スタートゾーンに戻る(アーム待機) + while(1) { + //printf("a-T = 2 T2 = %d t2_r = %d\n\r",T2,t2_r); + wait(0.1); + if(T2 == 1) { + break; + } + } + } + printf("end\n\r"); +} +#endif +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Sat Dec 14 12:42:13 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file