motorAB
Dependencies: mbed ros_lib_melodic
main.cpp
- Committer:
- la00noix
- Date:
- 2020-04-03
- Revision:
- 0:cca61e773cbb
- Child:
- 1:163629043391
File content as of revision 0:cca61e773cbb:
#include "mbed.h" #include "EC.h" #include <ros.h> //#include <geometry_msgs/Inertia.h> #include <geometry_msgs/Point.h> #include <geometry_msgs/Quaternion.h> #include <std_msgs/Int8.h> SpeedControl motorA(p7,p6,NC,360,0.05,p21,p22); SpeedControl motorB(p10,p11,NC,360,0.05,p23,p24); DigitalOut led(LED1); Ticker ticker; Ticker ticker2; Timer timer; double vA=0,vB=0; bool waiting=0; double time1; int k=0; int check_k=0; void calOmega(){ motorA.CalOmega(); motorB.CalOmega(); } ros::NodeHandle nh; //bool tickerstop=1; //void messageCallback(const geometry_msgs::Inertia& msg){ void messageCallback(const geometry_msgs::Point& msg){ vA=msg.x; vB=msg.y; bool reset=msg.z; if(reset){ vA=0; vB=0; motorA.reset(); motorB.reset(); } if(waiting==1){ timer.start(); waiting=0; }else{ time1=timer.read(); } } //ros::Subscriber<geometry_msgs::Inertia> motion_sub("motion",&messageCallback); ros::Subscriber<geometry_msgs::Point> motion_sub("motion_motorAB",&messageCallback); geometry_msgs::Quaternion data; ros::Publisher debug_pub("debug_duty_ab", &data); std_msgs::Int8 msg_main; ros::Publisher pub("main_check",&msg_main); void debug(){ data.x=motorA.getOmega(); data.y=motorB.getOmega(); data.z=motorA.duty; data.w=motorB.duty; debug_pub.publish( &data ); } int k_1=0; void spin(){ /*k_1++; if(k_1>50){ k_1=0; debug(); }*/ debug(); check_k++; if(check_k>100){ check_k=0; } msg_main.data=check_k; pub.publish(&msg_main); nh.spinOnce(); } int main() { //nh.getHardware()->setBaud(921600); nh.getHardware()->setBaud(115200); nh.initNode(); nh.subscribe(motion_sub); nh.advertise(pub); nh.advertise(debug_pub); timer.start(); motorA.pwm_F_.period_us(100); motorA.pwm_B_.period_us(100); motorB.pwm_F_.period_us(100); motorB.pwm_B_.period_us(100); //速度に対するpid係数 /*motorA.setPDparam(0.1,0.001); motorB.setPDparam(0.1,0.001);*/ motorA.setPDparam(0.09,0); motorB.setPDparam(0.05,0); //Cは傾き,Dは切片(縦デューティー,横入力値 のグラフにおいて) C=1/c, D=d motorA.setDOconstant(295,0,295,0); motorB.setDOconstant(305,0,305,0); //合わせたver /*motorA.setDOconstant(250,0,256,0); motorB.setDOconstant(252,0,267,0);*/ ticker.attach(&calOmega,0.05); wait_ms(5); ticker2.attach(&spin,0.01); while(1) { if(vA)motorA.Sc(vA); else motorA.stop(); if(vB)motorB.Sc(vB); else motorB.stop(); k++; if((k>5000)&&(waiting==0)){ double time_a=timer.read()-time1; if(time_a>0.5){ led=0; vA=0; vB=0; motorA.reset(); motorB.reset(); waiting=1; timer.stop(); timer.reset(); time1=0; }else{ led=1; } } } }