K AKAGAWA / Mbed 2 deprecated F446_CAN_V7_MotorTest_20190415

V7's MotorTestProgram for F446

Dependencies:   mbed ros_lib_kinetic USBDevice

main.cpp@0:27046fed2426, 2019-02-13 (annotated)

Committer:
ksingyuunyuu
Date:
Wed Feb 13 09:31:05 2019 +0000
Revision:
0:27046fed2426
Child:
1:417488dfc3d0
Kensiro

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ksingyuunyuu 0:27046fed2426 1 /*
ksingyuunyuu 0:27046fed2426 2 * rosserial Motor Shield Example
ksingyuunyuu 0:27046fed2426 3 *
ksingyuunyuu 0:27046fed2426 4 * This tutorial demonstrates the usage of the
ksingyuunyuu 0:27046fed2426 5 * Seeedstudio Motor Shield
ksingyuunyuu 0:27046fed2426 6 * http://www.seeedstudio.com/depot/motor-shield-v20-p-1377.html
ksingyuunyuu 0:27046fed2426 7 *
ksingyuunyuu 0:27046fed2426 8 * Source Code Based on:
ksingyuunyuu 0:27046fed2426 9 * https://developer.mbed.org/teams/shields/code/Seeed_Motor_Shield/
ksingyuunyuu 0:27046fed2426 10 */
ksingyuunyuu 0:27046fed2426 11
ksingyuunyuu 0:27046fed2426 12 #include "mbed.h"
ksingyuunyuu 0:27046fed2426 13 #include <ros.h>
ksingyuunyuu 0:27046fed2426 14 #include <geometry_msgs/Twist.h>
ksingyuunyuu 0:27046fed2426 15 #include "QEI.h"
ksingyuunyuu 0:27046fed2426 16 #include <std_msgs/String.h>
ksingyuunyuu 0:27046fed2426 17
ksingyuunyuu 0:27046fed2426 18 // マクロ定義(ピン配置)
ksingyuunyuu 0:27046fed2426 19 #define D0 PA_3
ksingyuunyuu 0:27046fed2426 20 #define D1 PA_2
ksingyuunyuu 0:27046fed2426 21 #define D2 PA_10
ksingyuunyuu 0:27046fed2426 22 #define D3 PB_3
ksingyuunyuu 0:27046fed2426 23 #define D4 PB_5
ksingyuunyuu 0:27046fed2426 24 #define D5 PB_4
ksingyuunyuu 0:27046fed2426 25 #define D6 PB_10
ksingyuunyuu 0:27046fed2426 26 #define D7 PA_8
ksingyuunyuu 0:27046fed2426 27 #define D8 PA_9
ksingyuunyuu 0:27046fed2426 28 #define D9 PC_7
ksingyuunyuu 0:27046fed2426 29 #define D10 PB_6
ksingyuunyuu 0:27046fed2426 30 #define D11 PA_7
ksingyuunyuu 0:27046fed2426 31 #define D12 PA_6
ksingyuunyuu 0:27046fed2426 32 #define D13 PA_5
ksingyuunyuu 0:27046fed2426 33 // アナログ
ksingyuunyuu 0:27046fed2426 34 #define A0 PA_0
ksingyuunyuu 0:27046fed2426 35 #define A1 PA_1
ksingyuunyuu 0:27046fed2426 36 #define A2 PA_4
ksingyuunyuu 0:27046fed2426 37 #define A3 PB_0
ksingyuunyuu 0:27046fed2426 38 #define A4 PC_1
ksingyuunyuu 0:27046fed2426 39 #define A5 PC_0
ksingyuunyuu 0:27046fed2426 40
ksingyuunyuu 0:27046fed2426 41 #define B1 PC_13
ksingyuunyuu 0:27046fed2426 42 // その他マクロ定義
ksingyuunyuu 0:27046fed2426 43 #define SAMPLING_TIME 0.01 // 100Hz
ksingyuunyuu 0:27046fed2426 44 #define ENC_PULSE_PER_METER 1965
ksingyuunyuu 0:27046fed2426 45 #define KP_LEFT 0.70
ksingyuunyuu 0:27046fed2426 46 #define KI_LEFT 0
ksingyuunyuu 0:27046fed2426 47 #define KD_LEFT 0
ksingyuunyuu 0:27046fed2426 48 #define KP_RIGHT 0.75
ksingyuunyuu 0:27046fed2426 49 #define KI_RIGHT 0
ksingyuunyuu 0:27046fed2426 50 #define KD_RIGHT 0
ksingyuunyuu 0:27046fed2426 51 #define SPEED_LEFT (double)(get_enc_left / 19.65)
ksingyuunyuu 0:27046fed2426 52 #define SPEED_RIGHT (double)(get_enc_right / 19.65)
ksingyuunyuu 0:27046fed2426 53
ksingyuunyuu 0:27046fed2426 54 // 入出力モード、各種モードの設定
ksingyuunyuu 0:27046fed2426 55 BusIn sw(B1);
ksingyuunyuu 0:27046fed2426 56 BusIn in(D5, D6, D11, D12);
ksingyuunyuu 0:27046fed2426 57 PwmOut motor_left(D4);
ksingyuunyuu 0:27046fed2426 58 PwmOut motor_right(D10);
ksingyuunyuu 0:27046fed2426 59 DigitalOut left(D3);
ksingyuunyuu 0:27046fed2426 60 DigitalOut right(D9);
ksingyuunyuu 0:27046fed2426 61 QEI enc_left(D5, D6, NC, 48, QEI::X4_ENCODING);
ksingyuunyuu 0:27046fed2426 62 QEI enc_right(D12, D11, NC, 48, QEI::X4_ENCODING);
ksingyuunyuu 0:27046fed2426 63 Ticker control;
ksingyuunyuu 0:27046fed2426 64 ros::NodeHandle nh;
ksingyuunyuu 0:27046fed2426 65
ksingyuunyuu 0:27046fed2426 66 // グローバル変数の宣言
ksingyuunyuu 0:27046fed2426 67 int get_enc_left;
ksingyuunyuu 0:27046fed2426 68 int get_enc_right;
ksingyuunyuu 0:27046fed2426 69 int enc_center;
ksingyuunyuu 0:27046fed2426 70 double SpeedIntegral_left = 0;
ksingyuunyuu 0:27046fed2426 71 double iSpeedBefore_left = 0;
ksingyuunyuu 0:27046fed2426 72 double TargetSpeed_left;
ksingyuunyuu 0:27046fed2426 73 double pwm_left;
ksingyuunyuu 0:27046fed2426 74 double SpeedIntegral_right = 0;
ksingyuunyuu 0:27046fed2426 75 double iSpeedBefore_right = 0;
ksingyuunyuu 0:27046fed2426 76 double TargetSpeed_right;
ksingyuunyuu 0:27046fed2426 77 double pwm_right;
ksingyuunyuu 0:27046fed2426 78 char str_left[13] = "mode left ";
ksingyuunyuu 0:27046fed2426 79 char str_right[13] = "mode right ";
ksingyuunyuu 0:27046fed2426 80 char str_forward[13] = "mode forward";
ksingyuunyuu 0:27046fed2426 81 char str_back[13] = "mode back ";
ksingyuunyuu 0:27046fed2426 82 char str_nutral[13] = "mode nutral ";
ksingyuunyuu 0:27046fed2426 83 // 関数のプロトタイプ宣言
ksingyuunyuu 0:27046fed2426 84 void function(void);
ksingyuunyuu 0:27046fed2426 85 void motor_speed_left( void );
ksingyuunyuu 0:27046fed2426 86 void motor_speed_right( void );
ksingyuunyuu 0:27046fed2426 87 void messageCb(const geometry_msgs::Twist& msg);
ksingyuunyuu 0:27046fed2426 88
ksingyuunyuu 0:27046fed2426 89 std_msgs::String str_msg;
ksingyuunyuu 0:27046fed2426 90 ros::Publisher chatter("chatter", &str_msg);
ksingyuunyuu 0:27046fed2426 91 ros::Subscriber<geometry_msgs::Twist> sub("diff_drive_controller/cmd_vel", &messageCb);
ksingyuunyuu 0:27046fed2426 92
ksingyuunyuu 0:27046fed2426 93 Timer t;
ksingyuunyuu 0:27046fed2426 94
ksingyuunyuu 0:27046fed2426 95 int main()
ksingyuunyuu 0:27046fed2426 96 {
ksingyuunyuu 0:27046fed2426 97
ksingyuunyuu 0:27046fed2426 98 in.mode(PullUp);
ksingyuunyuu 0:27046fed2426 99 control.attach(&function, SAMPLING_TIME); //台車の速度制御用のタイマー関数を設定
ksingyuunyuu 0:27046fed2426 100
ksingyuunyuu 0:27046fed2426 101 t.start();
ksingyuunyuu 0:27046fed2426 102 long vel_timer = 0;
ksingyuunyuu 0:27046fed2426 103 nh.getHardware()->setBaud(57600);
ksingyuunyuu 0:27046fed2426 104 nh.initNode();
ksingyuunyuu 0:27046fed2426 105 nh.subscribe(sub);
ksingyuunyuu 0:27046fed2426 106 nh.advertise(chatter);
ksingyuunyuu 0:27046fed2426 107 while (1)
ksingyuunyuu 0:27046fed2426 108 {
ksingyuunyuu 0:27046fed2426 109 if (t.read_ms() > vel_timer)
ksingyuunyuu 0:27046fed2426 110 {
ksingyuunyuu 0:27046fed2426 111 // 停止
ksingyuunyuu 0:27046fed2426 112 TargetSpeed_left = 0;
ksingyuunyuu 0:27046fed2426 113 TargetSpeed_right = 0;
ksingyuunyuu 0:27046fed2426 114 motor_left = 0;
ksingyuunyuu 0:27046fed2426 115 motor_right = 0;
ksingyuunyuu 0:27046fed2426 116 vel_timer = t.read_ms() + 500; // 500ms毎に実行
ksingyuunyuu 0:27046fed2426 117 }
ksingyuunyuu 0:27046fed2426 118 nh.spinOnce();
ksingyuunyuu 0:27046fed2426 119 motor_left = pwm_left;
ksingyuunyuu 0:27046fed2426 120 motor_right = pwm_right;
ksingyuunyuu 0:27046fed2426 121 wait_ms(1);
ksingyuunyuu 0:27046fed2426 122 }
ksingyuunyuu 0:27046fed2426 123 }
ksingyuunyuu 0:27046fed2426 124
ksingyuunyuu 0:27046fed2426 125 // 割り込み関数
ksingyuunyuu 0:27046fed2426 126 void function(void){
ksingyuunyuu 0:27046fed2426 127 motor_speed_left();
ksingyuunyuu 0:27046fed2426 128 motor_speed_right();
ksingyuunyuu 0:27046fed2426 129 }
ksingyuunyuu 0:27046fed2426 130
ksingyuunyuu 0:27046fed2426 131 void motor_speed_left( void ){
ksingyuunyuu 0:27046fed2426 132 double out;
ksingyuunyuu 0:27046fed2426 133 get_enc_left = enc_left.getPulses();
ksingyuunyuu 0:27046fed2426 134 enc_left.reset();
ksingyuunyuu 0:27046fed2426 135 double div_speed_left = TargetSpeed_left - SPEED_LEFT;
ksingyuunyuu 0:27046fed2426 136 out = KP_LEFT * div_speed_left;
ksingyuunyuu 0:27046fed2426 137 if(out < 0){
ksingyuunyuu 0:27046fed2426 138 out *= -1;
ksingyuunyuu 0:27046fed2426 139 left = 1;
ksingyuunyuu 0:27046fed2426 140 } else {
ksingyuunyuu 0:27046fed2426 141 left = 0;
ksingyuunyuu 0:27046fed2426 142 }
ksingyuunyuu 0:27046fed2426 143 pwm_left = out;
ksingyuunyuu 0:27046fed2426 144 }
ksingyuunyuu 0:27046fed2426 145 void motor_speed_right( void ){
ksingyuunyuu 0:27046fed2426 146 double out;
ksingyuunyuu 0:27046fed2426 147 get_enc_right = enc_right.getPulses();
ksingyuunyuu 0:27046fed2426 148 enc_right.reset();
ksingyuunyuu 0:27046fed2426 149 double div_speed_right = TargetSpeed_right - SPEED_RIGHT;
ksingyuunyuu 0:27046fed2426 150 out = KP_RIGHT * div_speed_right;
ksingyuunyuu 0:27046fed2426 151 if(out < 0){
ksingyuunyuu 0:27046fed2426 152 out *= -1;
ksingyuunyuu 0:27046fed2426 153 right = 1;
ksingyuunyuu 0:27046fed2426 154 } else {
ksingyuunyuu 0:27046fed2426 155 right = 0;
ksingyuunyuu 0:27046fed2426 156 }
ksingyuunyuu 0:27046fed2426 157 pwm_right = out;
ksingyuunyuu 0:27046fed2426 158 }
ksingyuunyuu 0:27046fed2426 159
ksingyuunyuu 0:27046fed2426 160 void messageCb(const geometry_msgs::Twist& msg){
ksingyuunyuu 0:27046fed2426 161 double tmp = 0;
ksingyuunyuu 0:27046fed2426 162
ksingyuunyuu 0:27046fed2426 163 TargetSpeed_left = msg.linear.x;
ksingyuunyuu 0:27046fed2426 164 TargetSpeed_right = msg.linear.x;
ksingyuunyuu 0:27046fed2426 165 tmp = 0;
ksingyuunyuu 0:27046fed2426 166
ksingyuunyuu 0:27046fed2426 167 if(msg.angular.z < 0){
ksingyuunyuu 0:27046fed2426 168 tmp = 1 - (-msg.angular.z / 3);
ksingyuunyuu 0:27046fed2426 169 TargetSpeed_left = msg.linear.x;
ksingyuunyuu 0:27046fed2426 170 TargetSpeed_right = msg.linear.x * tmp;
ksingyuunyuu 0:27046fed2426 171 } else if(msg.angular.z > 0){
ksingyuunyuu 0:27046fed2426 172 tmp = 1 - (msg.angular.z / 3);
ksingyuunyuu 0:27046fed2426 173 TargetSpeed_left = msg.linear.x * tmp;
ksingyuunyuu 0:27046fed2426 174 TargetSpeed_right = msg.linear.x;
ksingyuunyuu 0:27046fed2426 175 } else {
ksingyuunyuu 0:27046fed2426 176 TargetSpeed_right = msg.linear.x;
ksingyuunyuu 0:27046fed2426 177 TargetSpeed_right = msg.linear.x;
ksingyuunyuu 0:27046fed2426 178 }
ksingyuunyuu 0:27046fed2426 179 motor_left = pwm_left;
ksingyuunyuu 0:27046fed2426 180 motor_right = pwm_right;
ksingyuunyuu 0:27046fed2426 181
ksingyuunyuu 0:27046fed2426 182 /*
ksingyuunyuu 0:27046fed2426 183 if (msg.angular.z == 0 && msg.linear.x == 0){
ksingyuunyuu 0:27046fed2426 184 // 停止
ksingyuunyuu 0:27046fed2426 185 TargetSpeed_left = 0;
ksingyuunyuu 0:27046fed2426 186 TargetSpeed_right = 0;
ksingyuunyuu 0:27046fed2426 187 motor_left = pwm_left;
ksingyuunyuu 0:27046fed2426 188 motor_right = pwm_right;
ksingyuunyuu 0:27046fed2426 189 str_msg.data = str_nutral;
ksingyuunyuu 0:27046fed2426 190 chatter.publish( &str_msg );
ksingyuunyuu 0:27046fed2426 191 } else {
ksingyuunyuu 0:27046fed2426 192 if (msg.angular.z < 0){
ksingyuunyuu 0:27046fed2426 193 // 右旋回
ksingyuunyuu 0:27046fed2426 194 TargetSpeed_left = 1.0;
ksingyuunyuu 0:27046fed2426 195 TargetSpeed_right = 0;
ksingyuunyuu 0:27046fed2426 196 motor_left = pwm_left;
ksingyuunyuu 0:27046fed2426 197 motor_right = pwm_right;
ksingyuunyuu 0:27046fed2426 198 str_msg.data = str_right;
ksingyuunyuu 0:27046fed2426 199 chatter.publish( &str_msg );
ksingyuunyuu 0:27046fed2426 200 } else if (msg.angular.z > 0){
ksingyuunyuu 0:27046fed2426 201 // 左旋回
ksingyuunyuu 0:27046fed2426 202 TargetSpeed_left = 0;
ksingyuunyuu 0:27046fed2426 203 TargetSpeed_right = 1.0;
ksingyuunyuu 0:27046fed2426 204 motor_left = pwm_left;
ksingyuunyuu 0:27046fed2426 205 motor_right = pwm_right;
ksingyuunyuu 0:27046fed2426 206 str_msg.data = str_left;
ksingyuunyuu 0:27046fed2426 207 chatter.publish( &str_msg );
ksingyuunyuu 0:27046fed2426 208 } else if (msg.linear.x < 0){
ksingyuunyuu 0:27046fed2426 209 // 後進
ksingyuunyuu 0:27046fed2426 210 TargetSpeed_left = -1.0;
ksingyuunyuu 0:27046fed2426 211 TargetSpeed_right = -1.0;
ksingyuunyuu 0:27046fed2426 212 motor_left = pwm_left;
ksingyuunyuu 0:27046fed2426 213 motor_right = pwm_right;
ksingyuunyuu 0:27046fed2426 214 str_msg.data = str_back;
ksingyuunyuu 0:27046fed2426 215 chatter.publish( &str_msg );
ksingyuunyuu 0:27046fed2426 216 } else if (msg.linear.x > 0){
ksingyuunyuu 0:27046fed2426 217 // 前進
ksingyuunyuu 0:27046fed2426 218 TargetSpeed_left = 1.0;
ksingyuunyuu 0:27046fed2426 219 TargetSpeed_right = 1.0;
ksingyuunyuu 0:27046fed2426 220 motor_left = pwm_left;
ksingyuunyuu 0:27046fed2426 221 motor_right = pwm_right;
ksingyuunyuu 0:27046fed2426 222 str_msg.data = str_forward;
ksingyuunyuu 0:27046fed2426 223 chatter.publish( &str_msg );
ksingyuunyuu 0:27046fed2426 224 }
ksingyuunyuu 0:27046fed2426 225 }
ksingyuunyuu 0:27046fed2426 226 */
ksingyuunyuu 0:27046fed2426 227 }