2022_gorobo_Ateam / Mbed 2 deprecated PIDcontrol_4

足回りプログラム

Dependencies:   mbed SBDBT arrc_mbed

main.cpp@0:4f39632d7a42, 2022-01-17 (annotated)

Committer:
kazumayamanaka
Date:
Mon Jan 17 02:40:16 2022 +0000
Revision:
0:4f39632d7a42
this program is to motor control;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kazumayamanaka 0:4f39632d7a42 1 //------INCLUDE-------
kazumayamanaka 0:4f39632d7a42 2 #include "mbed.h"
kazumayamanaka 0:4f39632d7a42 3 #include "rotary_inc.hpp"
kazumayamanaka 0:4f39632d7a42 4 #include "PIDcontrol.hpp"
kazumayamanaka 0:4f39632d7a42 5 #include "sbdbt.hpp"
kazumayamanaka 0:4f39632d7a42 6
kazumayamanaka 0:4f39632d7a42 7 //------CLASS_DEFINE------
kazumayamanaka 0:4f39632d7a42 8 Serial pc(USBTX,USBRX);
kazumayamanaka 0:4f39632d7a42 9
kazumayamanaka 0:4f39632d7a42 10 RotaryInc Ror1(PA_6,PA_7,0);
kazumayamanaka 0:4f39632d7a42 11 RotaryInc Ror2(PC_10,PC_11,0);
kazumayamanaka 0:4f39632d7a42 12 RotaryInc Ror3(PA_8,PA_9,0);
kazumayamanaka 0:4f39632d7a42 13 RotaryInc Ror4(PA_14,PA_15,0);
kazumayamanaka 0:4f39632d7a42 14
kazumayamanaka 0:4f39632d7a42 15 PIDcontrol pid_1;
kazumayamanaka 0:4f39632d7a42 16 PIDcontrol pid_2;
kazumayamanaka 0:4f39632d7a42 17 PIDcontrol pid_3;
kazumayamanaka 0:4f39632d7a42 18 PIDcontrol pid_4;
kazumayamanaka 0:4f39632d7a42 19
kazumayamanaka 0:4f39632d7a42 20 sbdbt sb(PA_0,PA_1);
kazumayamanaka 0:4f39632d7a42 21
kazumayamanaka 0:4f39632d7a42 22 //------VARIABLE-------
kazumayamanaka 0:4f39632d7a42 23 Timer one;
kazumayamanaka 0:4f39632d7a42 24 double val_pulse[4];
kazumayamanaka 0:4f39632d7a42 25 double val_spd[4];
kazumayamanaka 0:4f39632d7a42 26 double val_target[4];
kazumayamanaka 0:4f39632d7a42 27 double timer;
kazumayamanaka 0:4f39632d7a42 28 const double PI=3.1415926535897;
kazumayamanaka 0:4f39632d7a42 29 double theta,angle=45;
kazumayamanaka 0:4f39632d7a42 30 double Xvalue,Yvalue,Xvelocity,Yvelocity;
kazumayamanaka 0:4f39632d7a42 31 //------FUNCTION-------
kazumayamanaka 0:4f39632d7a42 32 void get_pulse(void);
kazumayamanaka 0:4f39632d7a42 33 void angular(void);
kazumayamanaka 0:4f39632d7a42 34
kazumayamanaka 0:4f39632d7a42 35 int main(){
kazumayamanaka 0:4f39632d7a42 36 one.start();
kazumayamanaka 0:4f39632d7a42 37 while(1){
kazumayamanaka 0:4f39632d7a42 38 timer = one.read_us();
kazumayamanaka 0:4f39632d7a42 39 get_pulse();
kazumayamanaka 0:4f39632d7a42 40 angular();
kazumayamanaka 0:4f39632d7a42 41 //pid_a.motor_control(targetの値,pulseの値,PinName pin_a,PinName pin_b);
kazumayamanaka 0:4f39632d7a42 42 pid_1.motor_control(val_target[0],val_pulse[0],PB_14,PB_13);
kazumayamanaka 0:4f39632d7a42 43 pid_2.motor_control(val_target[1],val_pulse[1],PC_9,PC_8);
kazumayamanaka 0:4f39632d7a42 44 pid_3.motor_control(val_target[2],val_pulse[2],PB_5,PB_4);
kazumayamanaka 0:4f39632d7a42 45 pid_4.motor_control(val_target[3],val_pulse[3],PA_11,PB_1);
kazumayamanaka 0:4f39632d7a42 46
kazumayamanaka 0:4f39632d7a42 47 val_spd[0]=pid_1.get_spd();
kazumayamanaka 0:4f39632d7a42 48 val_spd[1]=pid_2.get_spd();
kazumayamanaka 0:4f39632d7a42 49 val_spd[2]=pid_3.get_spd();
kazumayamanaka 0:4f39632d7a42 50 val_spd[3]=pid_4.get_spd();
kazumayamanaka 0:4f39632d7a42 51
kazumayamanaka 0:4f39632d7a42 52 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]);
kazumayamanaka 0:4f39632d7a42 53 while(one.read_us() - timer <= 50 * 1000); //0.05s周期
kazumayamanaka 0:4f39632d7a42 54 }
kazumayamanaka 0:4f39632d7a42 55 }
kazumayamanaka 0:4f39632d7a42 56
kazumayamanaka 0:4f39632d7a42 57 //-------FUNCTION_DEFINE------
kazumayamanaka 0:4f39632d7a42 58 void get_pulse(){
kazumayamanaka 0:4f39632d7a42 59 val_pulse[0]=Ror1.get();
kazumayamanaka 0:4f39632d7a42 60 val_pulse[1]=Ror2.get();
kazumayamanaka 0:4f39632d7a42 61 val_pulse[2]=Ror3.get();
kazumayamanaka 0:4f39632d7a42 62 val_pulse[3]=Ror4.get();
kazumayamanaka 0:4f39632d7a42 63 }
kazumayamanaka 0:4f39632d7a42 64 void angular(){
kazumayamanaka 0:4f39632d7a42 65 theta=(angle/180.0)*PI;
kazumayamanaka 0:4f39632d7a42 66 if(sb.rsx()!=0||sb.rsx()!=0){
kazumayamanaka 0:4f39632d7a42 67 Xvalue=(sb.rsx()-64)*500/64.0;
kazumayamanaka 0:4f39632d7a42 68 Yvalue=(sb.rsy()-64)*500/64.0;
kazumayamanaka 0:4f39632d7a42 69 Xvelocity=Xvalue*cos(theta)-Yvalue*sin(theta);
kazumayamanaka 0:4f39632d7a42 70 Yvelocity=Xvalue*sin(theta)+Yvalue*cos(theta);
kazumayamanaka 0:4f39632d7a42 71 val_target[0]=-Xvelocity;
kazumayamanaka 0:4f39632d7a42 72 val_target[1]=-Yvelocity;
kazumayamanaka 0:4f39632d7a42 73 val_target[2]=Xvelocity;
kazumayamanaka 0:4f39632d7a42 74 val_target[3]=Yvelocity;
kazumayamanaka 0:4f39632d7a42 75 }
kazumayamanaka 0:4f39632d7a42 76 if(sb.l2()!=0){
kazumayamanaka 0:4f39632d7a42 77 Yvalue=sb.l2()*500;
kazumayamanaka 0:4f39632d7a42 78 for(int i=0;i<=3;i++){
kazumayamanaka 0:4f39632d7a42 79 val_target[i]=-Yvalue;
kazumayamanaka 0:4f39632d7a42 80 }
kazumayamanaka 0:4f39632d7a42 81 }
kazumayamanaka 0:4f39632d7a42 82 if(sb.r2()!=0){
kazumayamanaka 0:4f39632d7a42 83 Yvalue=sb.r2()*500;
kazumayamanaka 0:4f39632d7a42 84 for(int i=0;i<=3;i++){
kazumayamanaka 0:4f39632d7a42 85 val_target[i]=Yvalue;
kazumayamanaka 0:4f39632d7a42 86 }
kazumayamanaka 0:4f39632d7a42 87 }
kazumayamanaka 0:4f39632d7a42 88 }