V7's MotorTestProgram for F446
Dependencies: mbed ros_lib_kinetic USBDevice
main.cpp
- Committer:
- ksingyuunyuu
- Date:
- 2019-02-13
- Revision:
- 0:27046fed2426
- Child:
- 1:417488dfc3d0
File content as of revision 0:27046fed2426:
/* * rosserial Motor Shield Example * * This tutorial demonstrates the usage of the * Seeedstudio Motor Shield * http://www.seeedstudio.com/depot/motor-shield-v20-p-1377.html * * Source Code Based on: * https://developer.mbed.org/teams/shields/code/Seeed_Motor_Shield/ */ #include "mbed.h" #include <ros.h> #include <geometry_msgs/Twist.h> #include "QEI.h" #include <std_msgs/String.h> // マクロ定義(ピン配置) #define D0 PA_3 #define D1 PA_2 #define D2 PA_10 #define D3 PB_3 #define D4 PB_5 #define D5 PB_4 #define D6 PB_10 #define D7 PA_8 #define D8 PA_9 #define D9 PC_7 #define D10 PB_6 #define D11 PA_7 #define D12 PA_6 #define D13 PA_5 // アナログ #define A0 PA_0 #define A1 PA_1 #define A2 PA_4 #define A3 PB_0 #define A4 PC_1 #define A5 PC_0 #define B1 PC_13 // その他マクロ定義 #define SAMPLING_TIME 0.01 // 100Hz #define ENC_PULSE_PER_METER 1965 #define KP_LEFT 0.70 #define KI_LEFT 0 #define KD_LEFT 0 #define KP_RIGHT 0.75 #define KI_RIGHT 0 #define KD_RIGHT 0 #define SPEED_LEFT (double)(get_enc_left / 19.65) #define SPEED_RIGHT (double)(get_enc_right / 19.65) // 入出力モード、各種モードの設定 BusIn sw(B1); BusIn in(D5, D6, D11, D12); PwmOut motor_left(D4); PwmOut motor_right(D10); DigitalOut left(D3); DigitalOut right(D9); QEI enc_left(D5, D6, NC, 48, QEI::X4_ENCODING); QEI enc_right(D12, D11, NC, 48, QEI::X4_ENCODING); Ticker control; ros::NodeHandle nh; // グローバル変数の宣言 int get_enc_left; int get_enc_right; int enc_center; double SpeedIntegral_left = 0; double iSpeedBefore_left = 0; double TargetSpeed_left; double pwm_left; double SpeedIntegral_right = 0; double iSpeedBefore_right = 0; double TargetSpeed_right; double pwm_right; char str_left[13] = "mode left "; char str_right[13] = "mode right "; char str_forward[13] = "mode forward"; char str_back[13] = "mode back "; char str_nutral[13] = "mode nutral "; // 関数のプロトタイプ宣言 void function(void); void motor_speed_left( void ); void motor_speed_right( void ); void messageCb(const geometry_msgs::Twist& msg); std_msgs::String str_msg; ros::Publisher chatter("chatter", &str_msg); ros::Subscriber<geometry_msgs::Twist> sub("diff_drive_controller/cmd_vel", &messageCb); Timer t; int main() { in.mode(PullUp); control.attach(&function, SAMPLING_TIME); //台車の速度制御用のタイマー関数を設定 t.start(); long vel_timer = 0; nh.getHardware()->setBaud(57600); nh.initNode(); nh.subscribe(sub); nh.advertise(chatter); while (1) { if (t.read_ms() > vel_timer) { // 停止 TargetSpeed_left = 0; TargetSpeed_right = 0; motor_left = 0; motor_right = 0; vel_timer = t.read_ms() + 500; // 500ms毎に実行 } nh.spinOnce(); motor_left = pwm_left; motor_right = pwm_right; wait_ms(1); } } // 割り込み関数 void function(void){ motor_speed_left(); motor_speed_right(); } void motor_speed_left( void ){ double out; get_enc_left = enc_left.getPulses(); enc_left.reset(); double div_speed_left = TargetSpeed_left - SPEED_LEFT; out = KP_LEFT * div_speed_left; if(out < 0){ out *= -1; left = 1; } else { left = 0; } pwm_left = out; } void motor_speed_right( void ){ double out; get_enc_right = enc_right.getPulses(); enc_right.reset(); double div_speed_right = TargetSpeed_right - SPEED_RIGHT; out = KP_RIGHT * div_speed_right; if(out < 0){ out *= -1; right = 1; } else { right = 0; } pwm_right = out; } void messageCb(const geometry_msgs::Twist& msg){ double tmp = 0; TargetSpeed_left = msg.linear.x; TargetSpeed_right = msg.linear.x; tmp = 0; if(msg.angular.z < 0){ tmp = 1 - (-msg.angular.z / 3); TargetSpeed_left = msg.linear.x; TargetSpeed_right = msg.linear.x * tmp; } else if(msg.angular.z > 0){ tmp = 1 - (msg.angular.z / 3); TargetSpeed_left = msg.linear.x * tmp; TargetSpeed_right = msg.linear.x; } else { TargetSpeed_right = msg.linear.x; TargetSpeed_right = msg.linear.x; } motor_left = pwm_left; motor_right = pwm_right; /* if (msg.angular.z == 0 && msg.linear.x == 0){ // 停止 TargetSpeed_left = 0; TargetSpeed_right = 0; motor_left = pwm_left; motor_right = pwm_right; str_msg.data = str_nutral; chatter.publish( &str_msg ); } else { if (msg.angular.z < 0){ // 右旋回 TargetSpeed_left = 1.0; TargetSpeed_right = 0; motor_left = pwm_left; motor_right = pwm_right; str_msg.data = str_right; chatter.publish( &str_msg ); } else if (msg.angular.z > 0){ // 左旋回 TargetSpeed_left = 0; TargetSpeed_right = 1.0; motor_left = pwm_left; motor_right = pwm_right; str_msg.data = str_left; chatter.publish( &str_msg ); } else if (msg.linear.x < 0){ // 後進 TargetSpeed_left = -1.0; TargetSpeed_right = -1.0; motor_left = pwm_left; motor_right = pwm_right; str_msg.data = str_back; chatter.publish( &str_msg ); } else if (msg.linear.x > 0){ // 前進 TargetSpeed_left = 1.0; TargetSpeed_right = 1.0; motor_left = pwm_left; motor_right = pwm_right; str_msg.data = str_forward; chatter.publish( &str_msg ); } } */ }