Yulin Chen / Mbed 2 deprecated SOLamr_move_driver

This repo is for driving a differential-drived robot.

Dependencies:   mbed ros_lib_kinetic_1 Encoder

main.cpp@0:b21301d612b9, 2021-03-10 (annotated)

Committer:
r08522622
Date:
Wed Mar 10 03:23:54 2021 +0000
Revision:
0:b21301d612b9
test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
r08522622 0:b21301d612b9 1 #include "mbed.h"
r08522622 0:b21301d612b9 2 #include "ros.h"
r08522622 0:b21301d612b9 3 #include <std_msgs/String.h>
r08522622 0:b21301d612b9 4 #include <std_msgs/Float64.h>
r08522622 0:b21301d612b9 5 #include <geometry_msgs/Twist.h>
r08522622 0:b21301d612b9 6 #include "Encoder.h"
r08522622 0:b21301d612b9 7 #include "controller.h"
r08522622 0:b21301d612b9 8
r08522622 0:b21301d612b9 9 ros::NodeHandle nh;
r08522622 0:b21301d612b9 10 // Pin Out
r08522622 0:b21301d612b9 11 DigitalOut my_led(LED1);
r08522622 0:b21301d612b9 12
r08522622 0:b21301d612b9 13 PwmOut motor_left_blue(D10);
r08522622 0:b21301d612b9 14 PwmOut motor_left_green(PB_7);
r08522622 0:b21301d612b9 15
r08522622 0:b21301d612b9 16 PwmOut motor_right_blue(D8);
r08522622 0:b21301d612b9 17 PwmOut motor_right_green(D7);
r08522622 0:b21301d612b9 18
r08522622 0:b21301d612b9 19 // Define Publisher
r08522622 0:b21301d612b9 20 std_msgs::Float64 deg_left;
r08522622 0:b21301d612b9 21 std_msgs::Float64 deg_right;
r08522622 0:b21301d612b9 22 ros::Publisher encoder_left("/Encoder_left", &deg_left);
r08522622 0:b21301d612b9 23 ros::Publisher encoder_right("/Encoder_right", &deg_right);
r08522622 0:b21301d612b9 24
r08522622 0:b21301d612b9 25 // Test Publisher
r08522622 0:b21301d612b9 26 std_msgs::Float64 test;
r08522622 0:b21301d612b9 27 ros::Publisher Test("/Test", &test);
r08522622 0:b21301d612b9 28
r08522622 0:b21301d612b9 29 //STM mbed bug: these macros are MISSING from stm32f3xx_hal_tim.h
r08522622 0:b21301d612b9 30 #ifdef TARGET_STM32F4
r08522622 0:b21301d612b9 31 #define __HAL_TIM_GET_COUNTER(__HANDLE__) ((__HANDLE__)->Instance->CNT)
r08522622 0:b21301d612b9 32 #define __HAL_TIM_IS_TIM_COUNTING_DOWN(__HANDLE__) (((__HANDLE__)->Instance->CR1 &(TIM_CR1_DIR)) == (TIM_CR1_DIR))
r08522622 0:b21301d612b9 33 #endif
r08522622 0:b21301d612b9 34 TIM_Encoder_InitTypeDef encoder2;
r08522622 0:b21301d612b9 35 TIM_HandleTypeDef timer2;
r08522622 0:b21301d612b9 36 TIM_Encoder_InitTypeDef encoder1;
r08522622 0:b21301d612b9 37 TIM_HandleTypeDef timer1;
r08522622 0:b21301d612b9 38
r08522622 0:b21301d612b9 39 // Define Subscriber
r08522622 0:b21301d612b9 40 double ref_left, ref_right;
r08522622 0:b21301d612b9 41 void get_cmd_vel( const geometry_msgs::Twist &cmd_vel ){
r08522622 0:b21301d612b9 42 double vx = cmd_vel.linear.x;
r08522622 0:b21301d612b9 43 double wz = cmd_vel.angular.z;
r08522622 0:b21301d612b9 44 ref_left = (vx - wz * 0.45 / 2) / 0.075 *180 / 3.1415926;
r08522622 0:b21301d612b9 45 ref_right = (vx + wz * 0.45 / 2) / 0.075 *180 / 3.1415926;
r08522622 0:b21301d612b9 46 }
r08522622 0:b21301d612b9 47 ros::Subscriber<geometry_msgs::Twist> cmd("/cmd_vel", get_cmd_vel );
r08522622 0:b21301d612b9 48
r08522622 0:b21301d612b9 49 // Define Motor Driver Functions
r08522622 0:b21301d612b9 50 float saturation(float cmd){
r08522622 0:b21301d612b9 51 float sig = cmd;
r08522622 0:b21301d612b9 52 if(sig >= 1){
r08522622 0:b21301d612b9 53 sig = 1;
r08522622 0:b21301d612b9 54 }
r08522622 0:b21301d612b9 55 else if(sig <= -1){
r08522622 0:b21301d612b9 56 sig = -1;
r08522622 0:b21301d612b9 57 }
r08522622 0:b21301d612b9 58 return sig;
r08522622 0:b21301d612b9 59 }
r08522622 0:b21301d612b9 60
r08522622 0:b21301d612b9 61 void motor_drive_left( float pwm_cmd ){
r08522622 0:b21301d612b9 62 if (pwm_cmd >= 0){
r08522622 0:b21301d612b9 63 motor_left_blue.write(pwm_cmd);
r08522622 0:b21301d612b9 64 motor_left_green.write(0);
r08522622 0:b21301d612b9 65 }
r08522622 0:b21301d612b9 66 else{
r08522622 0:b21301d612b9 67 motor_left_blue.write(0);
r08522622 0:b21301d612b9 68 motor_left_green.write(-pwm_cmd);
r08522622 0:b21301d612b9 69 }
r08522622 0:b21301d612b9 70
r08522622 0:b21301d612b9 71 }
r08522622 0:b21301d612b9 72 void motor_drive_right( float pwm_cmd ){
r08522622 0:b21301d612b9 73 if (pwm_cmd >= 0){
r08522622 0:b21301d612b9 74 motor_right_blue.write(pwm_cmd);
r08522622 0:b21301d612b9 75 motor_right_green.write(0);
r08522622 0:b21301d612b9 76 }
r08522622 0:b21301d612b9 77 else{
r08522622 0:b21301d612b9 78 motor_right_blue.write(0);
r08522622 0:b21301d612b9 79 motor_right_green.write(-pwm_cmd);
r08522622 0:b21301d612b9 80 }
r08522622 0:b21301d612b9 81 }
r08522622 0:b21301d612b9 82
r08522622 0:b21301d612b9 83
r08522622 0:b21301d612b9 84 // Define initial setup
r08522622 0:b21301d612b9 85 void init_setup(){
r08522622 0:b21301d612b9 86 // ROS initial setup
r08522622 0:b21301d612b9 87 nh.initNode();//
r08522622 0:b21301d612b9 88 nh.advertise(encoder_left);
r08522622 0:b21301d612b9 89 nh.advertise(encoder_right);
r08522622 0:b21301d612b9 90 nh.advertise(Test);
r08522622 0:b21301d612b9 91 // Encoder Init
r08522622 0:b21301d612b9 92 EncoderInit(&encoder2, &timer2, TIM2, 0xffffffff, TIM_ENCODERMODE_TI12);
r08522622 0:b21301d612b9 93 EncoderInit(&encoder1, &timer1, TIM3, 0xffffffff, TIM_ENCODERMODE_TI12);
r08522622 0:b21301d612b9 94 // Set pwm frequency
r08522622 0:b21301d612b9 95 motor_left_blue.period_ms(10); // 100 Hz
r08522622 0:b21301d612b9 96 motor_left_green.period_ms(10); // 100 Hz
r08522622 0:b21301d612b9 97 motor_right_blue.period_ms(10); // 100 Hz
r08522622 0:b21301d612b9 98 motor_right_green.period_ms(10); // 100 Hz
r08522622 0:b21301d612b9 99
r08522622 0:b21301d612b9 100 nh.subscribe(cmd);
r08522622 0:b21301d612b9 101 }
r08522622 0:b21301d612b9 102
r08522622 0:b21301d612b9 103 int32_t count_left = 0, count_left_last = 0;
r08522622 0:b21301d612b9 104 int32_t count_right = 0, count_right_last = 0;
r08522622 0:b21301d612b9 105 int32_t cycle_left = 0, cycle_right = 0;
r08522622 0:b21301d612b9 106 float enc_left = 0, enc_right = 0;
r08522622 0:b21301d612b9 107
r08522622 0:b21301d612b9 108 void get_cycle(){
r08522622 0:b21301d612b9 109 if (count_left - count_left_last > 55000){ //BW Cycle
r08522622 0:b21301d612b9 110 cycle_left -= 1;
r08522622 0:b21301d612b9 111 }
r08522622 0:b21301d612b9 112 else if (count_left - count_left_last < -55000){ //FW Cycle
r08522622 0:b21301d612b9 113 cycle_left += 1;
r08522622 0:b21301d612b9 114 }
r08522622 0:b21301d612b9 115 if (count_right - count_right_last > 55000){ //BW Cycle
r08522622 0:b21301d612b9 116 cycle_right -= 1;
r08522622 0:b21301d612b9 117 }
r08522622 0:b21301d612b9 118 else if (count_right - count_right_last < -55000){ //FW Cycle
r08522622 0:b21301d612b9 119 cycle_right += 1;
r08522622 0:b21301d612b9 120 }
r08522622 0:b21301d612b9 121 count_left_last = count_left;
r08522622 0:b21301d612b9 122 count_right_last = count_right;
r08522622 0:b21301d612b9 123 }
r08522622 0:b21301d612b9 124
r08522622 0:b21301d612b9 125
r08522622 0:b21301d612b9 126
r08522622 0:b21301d612b9 127
r08522622 0:b21301d612b9 128
r08522622 0:b21301d612b9 129 int main() {
r08522622 0:b21301d612b9 130 // Initialization
r08522622 0:b21301d612b9 131 init_setup();
r08522622 0:b21301d612b9 132
r08522622 0:b21301d612b9 133 // Define Class
r08522622 0:b21301d612b9 134 Ctrl Processor_left;
r08522622 0:b21301d612b9 135 Ctrl Processor_right;
r08522622 0:b21301d612b9 136
r08522622 0:b21301d612b9 137 // Initial Conditions of processor
r08522622 0:b21301d612b9 138 Processor_left.init();
r08522622 0:b21301d612b9 139 Processor_right.init();
r08522622 0:b21301d612b9 140 while (1) {
r08522622 0:b21301d612b9 141
r08522622 0:b21301d612b9 142 // Read Encoder Count
r08522622 0:b21301d612b9 143 // Count to Deg: deg = 360/960*count;
r08522622 0:b21301d612b9 144 count_left = __HAL_TIM_GET_COUNTER(&timer2);
r08522622 0:b21301d612b9 145 count_right = __HAL_TIM_GET_COUNTER(&timer1);
r08522622 0:b21301d612b9 146 get_cycle();
r08522622 0:b21301d612b9 147 enc_left = float(count_left + 65536*cycle_left ) *360/960;
r08522622 0:b21301d612b9 148 enc_right = float(count_right + 65536*cycle_right) *360/960;
r08522622 0:b21301d612b9 149 deg_left.data = enc_left;
r08522622 0:b21301d612b9 150 deg_right.data = enc_right;
r08522622 0:b21301d612b9 151
r08522622 0:b21301d612b9 152 encoder_left.publish( &deg_left );
r08522622 0:b21301d612b9 153 encoder_right.publish( &deg_right );
r08522622 0:b21301d612b9 154 /*
r08522622 0:b21301d612b9 155 motor_right_blue.write(0.1);
r08522622 0:b21301d612b9 156 motor_right_green.write(0.0);
r08522622 0:b21301d612b9 157
r08522622 0:b21301d612b9 158 motor_left_blue.write(0.1);
r08522622 0:b21301d612b9 159 motor_left_green.write(0.0);
r08522622 0:b21301d612b9 160 */
r08522622 0:b21301d612b9 161 // Get reference input
r08522622 0:b21301d612b9 162 Processor_left.ref = ref_left;
r08522622 0:b21301d612b9 163 Processor_right.ref = ref_right;
r08522622 0:b21301d612b9 164
r08522622 0:b21301d612b9 165 // Calculate volt command: duty = volt / 24 * 1;
r08522622 0:b21301d612b9 166 float gain_left = Processor_left.PID()/24*1;
r08522622 0:b21301d612b9 167 float sig_left = saturation(gain_left);
r08522622 0:b21301d612b9 168 motor_drive_left(sig_left);
r08522622 0:b21301d612b9 169
r08522622 0:b21301d612b9 170 float gain_right = Processor_right.PID()/24*1;
r08522622 0:b21301d612b9 171 float sig_right = saturation(gain_right);
r08522622 0:b21301d612b9 172 motor_drive_right(sig_right);
r08522622 0:b21301d612b9 173
r08522622 0:b21301d612b9 174 // test.data = ttt;
r08522622 0:b21301d612b9 175 // Test.publish(&test);
r08522622 0:b21301d612b9 176
r08522622 0:b21301d612b9 177 // Measurement and Filter
r08522622 0:b21301d612b9 178 Processor_left.theta = enc_left;
r08522622 0:b21301d612b9 179 Processor_left.omega_processor();
r08522622 0:b21301d612b9 180
r08522622 0:b21301d612b9 181 Processor_right.theta = enc_right;
r08522622 0:b21301d612b9 182 Processor_right.omega_processor();
r08522622 0:b21301d612b9 183
r08522622 0:b21301d612b9 184 nh.spinOnce();
r08522622 0:b21301d612b9 185 wait_ms(20);
r08522622 0:b21301d612b9 186 }
r08522622 0:b21301d612b9 187 }