勇紀 有岡
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