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?
User | Revision | Line number | New 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", °_left); |
r08522622 | 0:b21301d612b9 | 23 | ros::Publisher encoder_right("/Encoder_right", °_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( °_left ); |
r08522622 | 0:b21301d612b9 | 153 | encoder_right.publish( °_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 | } |