足回りプログラム
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?
User | Revision | Line number | New 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 | } |