motorAB
Dependencies: mbed ros_lib_melodic
main.cpp@0:cca61e773cbb, 2020-04-03 (annotated)
- Committer:
- la00noix
- Date:
- Fri Apr 03 02:52:21 2020 +0000
- Revision:
- 0:cca61e773cbb
- Child:
- 1:163629043391
a
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
la00noix | 0:cca61e773cbb | 1 | #include "mbed.h" |
la00noix | 0:cca61e773cbb | 2 | #include "EC.h" |
la00noix | 0:cca61e773cbb | 3 | #include <ros.h> |
la00noix | 0:cca61e773cbb | 4 | //#include <geometry_msgs/Inertia.h> |
la00noix | 0:cca61e773cbb | 5 | #include <geometry_msgs/Point.h> |
la00noix | 0:cca61e773cbb | 6 | #include <geometry_msgs/Quaternion.h> |
la00noix | 0:cca61e773cbb | 7 | #include <std_msgs/Int8.h> |
la00noix | 0:cca61e773cbb | 8 | |
la00noix | 0:cca61e773cbb | 9 | SpeedControl motorA(p7,p6,NC,360,0.05,p21,p22); |
la00noix | 0:cca61e773cbb | 10 | SpeedControl motorB(p10,p11,NC,360,0.05,p23,p24); |
la00noix | 0:cca61e773cbb | 11 | |
la00noix | 0:cca61e773cbb | 12 | |
la00noix | 0:cca61e773cbb | 13 | DigitalOut led(LED1); |
la00noix | 0:cca61e773cbb | 14 | |
la00noix | 0:cca61e773cbb | 15 | Ticker ticker; |
la00noix | 0:cca61e773cbb | 16 | Ticker ticker2; |
la00noix | 0:cca61e773cbb | 17 | Timer timer; |
la00noix | 0:cca61e773cbb | 18 | |
la00noix | 0:cca61e773cbb | 19 | double vA=0,vB=0; |
la00noix | 0:cca61e773cbb | 20 | bool waiting=0; |
la00noix | 0:cca61e773cbb | 21 | double time1; |
la00noix | 0:cca61e773cbb | 22 | |
la00noix | 0:cca61e773cbb | 23 | int k=0; |
la00noix | 0:cca61e773cbb | 24 | int check_k=0; |
la00noix | 0:cca61e773cbb | 25 | |
la00noix | 0:cca61e773cbb | 26 | void calOmega(){ |
la00noix | 0:cca61e773cbb | 27 | motorA.CalOmega(); |
la00noix | 0:cca61e773cbb | 28 | motorB.CalOmega(); |
la00noix | 0:cca61e773cbb | 29 | } |
la00noix | 0:cca61e773cbb | 30 | |
la00noix | 0:cca61e773cbb | 31 | ros::NodeHandle nh; |
la00noix | 0:cca61e773cbb | 32 | |
la00noix | 0:cca61e773cbb | 33 | //bool tickerstop=1; |
la00noix | 0:cca61e773cbb | 34 | |
la00noix | 0:cca61e773cbb | 35 | //void messageCallback(const geometry_msgs::Inertia& msg){ |
la00noix | 0:cca61e773cbb | 36 | void messageCallback(const geometry_msgs::Point& msg){ |
la00noix | 0:cca61e773cbb | 37 | |
la00noix | 0:cca61e773cbb | 38 | vA=msg.x; |
la00noix | 0:cca61e773cbb | 39 | vB=msg.y; |
la00noix | 0:cca61e773cbb | 40 | bool reset=msg.z; |
la00noix | 0:cca61e773cbb | 41 | |
la00noix | 0:cca61e773cbb | 42 | if(reset){ |
la00noix | 0:cca61e773cbb | 43 | vA=0; |
la00noix | 0:cca61e773cbb | 44 | vB=0; |
la00noix | 0:cca61e773cbb | 45 | motorA.reset(); |
la00noix | 0:cca61e773cbb | 46 | motorB.reset(); |
la00noix | 0:cca61e773cbb | 47 | } |
la00noix | 0:cca61e773cbb | 48 | if(waiting==1){ |
la00noix | 0:cca61e773cbb | 49 | timer.start(); |
la00noix | 0:cca61e773cbb | 50 | waiting=0; |
la00noix | 0:cca61e773cbb | 51 | }else{ |
la00noix | 0:cca61e773cbb | 52 | time1=timer.read(); |
la00noix | 0:cca61e773cbb | 53 | } |
la00noix | 0:cca61e773cbb | 54 | } |
la00noix | 0:cca61e773cbb | 55 | |
la00noix | 0:cca61e773cbb | 56 | //ros::Subscriber<geometry_msgs::Inertia> motion_sub("motion",&messageCallback); |
la00noix | 0:cca61e773cbb | 57 | ros::Subscriber<geometry_msgs::Point> motion_sub("motion_motorAB",&messageCallback); |
la00noix | 0:cca61e773cbb | 58 | |
la00noix | 0:cca61e773cbb | 59 | geometry_msgs::Quaternion data; |
la00noix | 0:cca61e773cbb | 60 | ros::Publisher debug_pub("debug_duty_ab", &data); |
la00noix | 0:cca61e773cbb | 61 | |
la00noix | 0:cca61e773cbb | 62 | std_msgs::Int8 msg_main; |
la00noix | 0:cca61e773cbb | 63 | ros::Publisher pub("main_check",&msg_main); |
la00noix | 0:cca61e773cbb | 64 | |
la00noix | 0:cca61e773cbb | 65 | |
la00noix | 0:cca61e773cbb | 66 | void debug(){ |
la00noix | 0:cca61e773cbb | 67 | data.x=motorA.getOmega(); |
la00noix | 0:cca61e773cbb | 68 | data.y=motorB.getOmega(); |
la00noix | 0:cca61e773cbb | 69 | data.z=motorA.duty; |
la00noix | 0:cca61e773cbb | 70 | data.w=motorB.duty; |
la00noix | 0:cca61e773cbb | 71 | debug_pub.publish( &data ); |
la00noix | 0:cca61e773cbb | 72 | } |
la00noix | 0:cca61e773cbb | 73 | |
la00noix | 0:cca61e773cbb | 74 | int k_1=0; |
la00noix | 0:cca61e773cbb | 75 | void spin(){ |
la00noix | 0:cca61e773cbb | 76 | /*k_1++; |
la00noix | 0:cca61e773cbb | 77 | if(k_1>50){ |
la00noix | 0:cca61e773cbb | 78 | k_1=0; |
la00noix | 0:cca61e773cbb | 79 | debug(); |
la00noix | 0:cca61e773cbb | 80 | }*/ |
la00noix | 0:cca61e773cbb | 81 | debug(); |
la00noix | 0:cca61e773cbb | 82 | check_k++; |
la00noix | 0:cca61e773cbb | 83 | if(check_k>100){ |
la00noix | 0:cca61e773cbb | 84 | check_k=0; |
la00noix | 0:cca61e773cbb | 85 | } |
la00noix | 0:cca61e773cbb | 86 | msg_main.data=check_k; |
la00noix | 0:cca61e773cbb | 87 | pub.publish(&msg_main); |
la00noix | 0:cca61e773cbb | 88 | nh.spinOnce(); |
la00noix | 0:cca61e773cbb | 89 | } |
la00noix | 0:cca61e773cbb | 90 | |
la00noix | 0:cca61e773cbb | 91 | int main() { |
la00noix | 0:cca61e773cbb | 92 | //nh.getHardware()->setBaud(921600); |
la00noix | 0:cca61e773cbb | 93 | nh.getHardware()->setBaud(115200); |
la00noix | 0:cca61e773cbb | 94 | nh.initNode(); |
la00noix | 0:cca61e773cbb | 95 | nh.subscribe(motion_sub); |
la00noix | 0:cca61e773cbb | 96 | nh.advertise(pub); |
la00noix | 0:cca61e773cbb | 97 | nh.advertise(debug_pub); |
la00noix | 0:cca61e773cbb | 98 | |
la00noix | 0:cca61e773cbb | 99 | timer.start(); |
la00noix | 0:cca61e773cbb | 100 | |
la00noix | 0:cca61e773cbb | 101 | motorA.pwm_F_.period_us(100); |
la00noix | 0:cca61e773cbb | 102 | motorA.pwm_B_.period_us(100); |
la00noix | 0:cca61e773cbb | 103 | motorB.pwm_F_.period_us(100); |
la00noix | 0:cca61e773cbb | 104 | motorB.pwm_B_.period_us(100); |
la00noix | 0:cca61e773cbb | 105 | |
la00noix | 0:cca61e773cbb | 106 | |
la00noix | 0:cca61e773cbb | 107 | |
la00noix | 0:cca61e773cbb | 108 | |
la00noix | 0:cca61e773cbb | 109 | |
la00noix | 0:cca61e773cbb | 110 | //速度に対するpid係数 |
la00noix | 0:cca61e773cbb | 111 | /*motorA.setPDparam(0.1,0.001); |
la00noix | 0:cca61e773cbb | 112 | motorB.setPDparam(0.1,0.001);*/ |
la00noix | 0:cca61e773cbb | 113 | motorA.setPDparam(0.09,0); |
la00noix | 0:cca61e773cbb | 114 | motorB.setPDparam(0.05,0); |
la00noix | 0:cca61e773cbb | 115 | |
la00noix | 0:cca61e773cbb | 116 | |
la00noix | 0:cca61e773cbb | 117 | //Cは傾き,Dは切片(縦デューティー,横入力値 のグラフにおいて) C=1/c, D=d |
la00noix | 0:cca61e773cbb | 118 | motorA.setDOconstant(295,0,295,0); |
la00noix | 0:cca61e773cbb | 119 | motorB.setDOconstant(305,0,305,0); |
la00noix | 0:cca61e773cbb | 120 | |
la00noix | 0:cca61e773cbb | 121 | |
la00noix | 0:cca61e773cbb | 122 | //合わせたver |
la00noix | 0:cca61e773cbb | 123 | /*motorA.setDOconstant(250,0,256,0); |
la00noix | 0:cca61e773cbb | 124 | motorB.setDOconstant(252,0,267,0);*/ |
la00noix | 0:cca61e773cbb | 125 | |
la00noix | 0:cca61e773cbb | 126 | |
la00noix | 0:cca61e773cbb | 127 | |
la00noix | 0:cca61e773cbb | 128 | |
la00noix | 0:cca61e773cbb | 129 | |
la00noix | 0:cca61e773cbb | 130 | ticker.attach(&calOmega,0.05); |
la00noix | 0:cca61e773cbb | 131 | wait_ms(5); |
la00noix | 0:cca61e773cbb | 132 | ticker2.attach(&spin,0.01); |
la00noix | 0:cca61e773cbb | 133 | |
la00noix | 0:cca61e773cbb | 134 | while(1) { |
la00noix | 0:cca61e773cbb | 135 | if(vA)motorA.Sc(vA); |
la00noix | 0:cca61e773cbb | 136 | else motorA.stop(); |
la00noix | 0:cca61e773cbb | 137 | if(vB)motorB.Sc(vB); |
la00noix | 0:cca61e773cbb | 138 | else motorB.stop(); |
la00noix | 0:cca61e773cbb | 139 | k++; |
la00noix | 0:cca61e773cbb | 140 | if((k>5000)&&(waiting==0)){ |
la00noix | 0:cca61e773cbb | 141 | double time_a=timer.read()-time1; |
la00noix | 0:cca61e773cbb | 142 | if(time_a>0.5){ |
la00noix | 0:cca61e773cbb | 143 | led=0; |
la00noix | 0:cca61e773cbb | 144 | vA=0; |
la00noix | 0:cca61e773cbb | 145 | vB=0; |
la00noix | 0:cca61e773cbb | 146 | motorA.reset(); |
la00noix | 0:cca61e773cbb | 147 | motorB.reset(); |
la00noix | 0:cca61e773cbb | 148 | |
la00noix | 0:cca61e773cbb | 149 | waiting=1; |
la00noix | 0:cca61e773cbb | 150 | timer.stop(); |
la00noix | 0:cca61e773cbb | 151 | timer.reset(); |
la00noix | 0:cca61e773cbb | 152 | time1=0; |
la00noix | 0:cca61e773cbb | 153 | }else{ |
la00noix | 0:cca61e773cbb | 154 | led=1; |
la00noix | 0:cca61e773cbb | 155 | } |
la00noix | 0:cca61e773cbb | 156 | } |
la00noix | 0:cca61e773cbb | 157 | } |
la00noix | 0:cca61e773cbb | 158 | } |