2022_gorobo_Ateam
足回りプログラム
Dependencies: mbed SBDBT arrc_mbed
Revision 0:4f39632d7a42, committed 2022-01-17
- Comitter:
- kazumayamanaka
- Date:
- Mon Jan 17 02:40:16 2022 +0000
- Commit message:
- this program is to motor control;
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/PIDcontrol.cpp Mon Jan 17 02:40:16 2022 +0000
@@ -0,0 +1,63 @@
+#include "PIDcontrol.hpp"
+
+//コンストラクタ
+PIDcontrol::PIDcontrol(){
+ Kp=0.0001;
+ Ki=0.0000014;
+ Kd=0.0000001;
+ spd=0.0;
+ pwm=0.0;
+ dt=0.05;
+ integral=0.0;
+ finish=0;
+ pal=0;
+}
+
+
+//spd計算
+void PIDcontrol::speed(){
+ finish = pulse - pal;
+ spd = co*finish/dt;
+ pal = pulse;
+}
+
+
+//pwm計算
+void PIDcontrol::control(){
+ error_n = target-spd;
+ integral += (error_n+error_b) / 2.0 * dt;
+ P = Kp * error_n;
+ I = Ki * integral;
+ D = Kd * (error_n-error_b) / dt;
+ pwm += (P+I+D);
+ error_b=error_n;
+}
+
+//モーター制御
+void PIDcontrol::motor_control(double var_1,double var_2,PinName pin_a,PinName pin_b){
+ PwmOut v1p(pin_a);
+ PwmOut v1m(pin_b);
+ v1p.period_us(2048);
+ v1m.period_us(2048);
+ target=var_1;
+ pulse=var_2;
+ speed();
+ control();
+ if(pwm>0){
+ v1p = pwm > 0.5 ? 0.5 : pwm;
+ v1m=0;
+ }else if(pwm<0){
+ v1p = 0;
+ v1m = -pwm > 0.5 ? 0.5 : -pwm;
+ }else{
+ v1p = 0;
+ v1m = 0;
+ }
+}
+
+//spdをreturnする
+double PIDcontrol::get_spd(){
+ return spd;
+}
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/PIDcontrol.hpp Mon Jan 17 02:40:16 2022 +0000
@@ -0,0 +1,28 @@
+#ifndef PID_CONTROL_H
+#define PID_CONTROL_H
+#include "mbed.h"
+#define co (108*3.1415926535897)/500
+
+class PIDcontrol{
+ public:
+ double spd;//速度
+ double error_n,error_b;//現在の誤差、過去の誤差
+ double P,I,D;//pid制御のそれぞれの値
+ double pwm,v1p_set,v1m_set;//モータに入れる値
+ double integral;//誤差の面積
+ double finish,pulse,pal;//パルス関係
+ double target;//目標値
+ PIDcontrol();//コンストラクタ
+ void motor_control(double var_1,double var_2,PinName pin_a,PinName pin_b);//pwmをreturnする関数
+ double get_spd();//spdをreturnする関数
+ private:
+ void speed();//spd計算
+ void control();//PID制御
+ double dt;//dt=0.05
+ double Kp,Ki,Kd;//PID制御
+};
+
+#endif
+
+
+
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SBDBT.lib Mon Jan 17 02:40:16 2022 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/m2130/code/SBDBT/#9fd7393a3023
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/arrc_mbed.lib Mon Jan 17 02:40:16 2022 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/TanakaRobo/code/arrc_mbed/#77c13e86ad12
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Mon Jan 17 02:40:16 2022 +0000
@@ -0,0 +1,88 @@
+//------INCLUDE-------
+#include "mbed.h"
+#include "rotary_inc.hpp"
+#include "PIDcontrol.hpp"
+#include "sbdbt.hpp"
+
+//------CLASS_DEFINE------
+Serial pc(USBTX,USBRX);
+
+RotaryInc Ror1(PA_6,PA_7,0);
+RotaryInc Ror2(PC_10,PC_11,0);
+RotaryInc Ror3(PA_8,PA_9,0);
+RotaryInc Ror4(PA_14,PA_15,0);
+
+PIDcontrol pid_1;
+PIDcontrol pid_2;
+PIDcontrol pid_3;
+PIDcontrol pid_4;
+
+sbdbt sb(PA_0,PA_1);
+
+//------VARIABLE-------
+Timer one;
+double val_pulse[4];
+double val_spd[4];
+double val_target[4];
+double timer;
+const double PI=3.1415926535897;
+double theta,angle=45;
+double Xvalue,Yvalue,Xvelocity,Yvelocity;
+//------FUNCTION-------
+void get_pulse(void);
+void angular(void);
+
+int main(){
+ one.start();
+ while(1){
+ timer = one.read_us();
+ get_pulse();
+ angular();
+ //pid_a.motor_control(targetの値,pulseの値,PinName pin_a,PinName pin_b);
+ pid_1.motor_control(val_target[0],val_pulse[0],PB_14,PB_13);
+ pid_2.motor_control(val_target[1],val_pulse[1],PC_9,PC_8);
+ pid_3.motor_control(val_target[2],val_pulse[2],PB_5,PB_4);
+ pid_4.motor_control(val_target[3],val_pulse[3],PA_11,PB_1);
+
+ val_spd[0]=pid_1.get_spd();
+ val_spd[1]=pid_2.get_spd();
+ val_spd[2]=pid_3.get_spd();
+ val_spd[3]=pid_4.get_spd();
+
+ pc.printf("%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf\n",val_target[0],val_target[1],val_target[2],val_target[3],val_spd[0],val_spd[1],val_spd[2],val_spd[3]);
+ while(one.read_us() - timer <= 50 * 1000); //0.05s周期
+ }
+}
+
+//-------FUNCTION_DEFINE------
+void get_pulse(){
+ val_pulse[0]=Ror1.get();
+ val_pulse[1]=Ror2.get();
+ val_pulse[2]=Ror3.get();
+ val_pulse[3]=Ror4.get();
+}
+void angular(){
+ theta=(angle/180.0)*PI;
+ if(sb.rsx()!=0||sb.rsx()!=0){
+ Xvalue=(sb.rsx()-64)*500/64.0;
+ Yvalue=(sb.rsy()-64)*500/64.0;
+ Xvelocity=Xvalue*cos(theta)-Yvalue*sin(theta);
+ Yvelocity=Xvalue*sin(theta)+Yvalue*cos(theta);
+ val_target[0]=-Xvelocity;
+ val_target[1]=-Yvelocity;
+ val_target[2]=Xvelocity;
+ val_target[3]=Yvelocity;
+ }
+ if(sb.l2()!=0){
+ Yvalue=sb.l2()*500;
+ for(int i=0;i<=3;i++){
+ val_target[i]=-Yvalue;
+ }
+ }
+ if(sb.r2()!=0){
+ Yvalue=sb.r2()*500;
+ for(int i=0;i<=3;i++){
+ val_target[i]=Yvalue;
+ }
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Mon Jan 17 02:40:16 2022 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file