This repo is for driving a differential-drived robot.
Dependencies: mbed ros_lib_kinetic_1 Encoder
Revision 0:b21301d612b9, committed 2021-03-10
- Comitter:
- r08522622
- Date:
- Wed Mar 10 03:23:54 2021 +0000
- Commit message:
- test
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Encoder.lib Wed Mar 10 03:23:54 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/r08522622/code/Encoder/#33aac5ba5a2b
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/controller.cpp Wed Mar 10 03:23:54 2021 +0000 @@ -0,0 +1,71 @@ +#include "controller.h" +Ctrl::Ctrl() { + +} + + +void Ctrl::init(){ + lp_w = 0; + error_last = 0; + volt = 0; + w0 = 0; + w1 = 0; + w2 = 0; + theta_last = 0; + dt = 0.02; +} + +float Ctrl::PID() { + error = ref - lp_w; + volt += error*0.018 - error_last*0.01689; + error_last = error; + return volt; +} + +float Ctrl::get_omega(){ + w2 = (theta - theta_last)/dt; + theta_last = theta; + return w2; +} + +float Ctrl::low_pass_filter(){ + lp_w = (w0 + 2*w1 + w2)/4; + w1 = w2; + w0 = w1; + return lp_w; +} + +void Ctrl::omega_processor(){ + Ctrl::get_omega(); + Ctrl::low_pass_filter(); +} + +/* + +int main(void) { + + Ctrl t; + t.ref = 11; + t.lp_w=0; + + while(1) { + t.ref = 11; + gain = t.PID(); + + if .... + endif + + t.theta_last = t.theta; + t.theta = counter /4; + + t.w0 = t.w1; + t.w1 = t.w2; + t.w2 = t.get_omega(); + t.lp_w = t.low_pass_filter(); + + } + + return 0; +} + +*/ \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/controller.h Wed Mar 10 03:23:54 2021 +0000 @@ -0,0 +1,18 @@ +class Ctrl { + + public: + Ctrl(); + float ref; + float error, error_last; + float volt; + float theta, theta_last; + float dt; + float w0, w1, w2, lp_w; + + void init(); + float PID(); + float get_omega(); + float low_pass_filter(); + void omega_processor(); + +}; \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Mar 10 03:23:54 2021 +0000 @@ -0,0 +1,187 @@ +#include "mbed.h" +#include "ros.h" +#include <std_msgs/String.h> +#include <std_msgs/Float64.h> +#include <geometry_msgs/Twist.h> +#include "Encoder.h" +#include "controller.h" + +ros::NodeHandle nh; +// Pin Out +DigitalOut my_led(LED1); + +PwmOut motor_left_blue(D10); +PwmOut motor_left_green(PB_7); + +PwmOut motor_right_blue(D8); +PwmOut motor_right_green(D7); + +// Define Publisher +std_msgs::Float64 deg_left; +std_msgs::Float64 deg_right; +ros::Publisher encoder_left("/Encoder_left", °_left); +ros::Publisher encoder_right("/Encoder_right", °_right); + +// Test Publisher +std_msgs::Float64 test; +ros::Publisher Test("/Test", &test); + +//STM mbed bug: these macros are MISSING from stm32f3xx_hal_tim.h +#ifdef TARGET_STM32F4 +#define __HAL_TIM_GET_COUNTER(__HANDLE__) ((__HANDLE__)->Instance->CNT) +#define __HAL_TIM_IS_TIM_COUNTING_DOWN(__HANDLE__) (((__HANDLE__)->Instance->CR1 &(TIM_CR1_DIR)) == (TIM_CR1_DIR)) +#endif +TIM_Encoder_InitTypeDef encoder2; +TIM_HandleTypeDef timer2; +TIM_Encoder_InitTypeDef encoder1; +TIM_HandleTypeDef timer1; + +// Define Subscriber +double ref_left, ref_right; +void get_cmd_vel( const geometry_msgs::Twist &cmd_vel ){ + double vx = cmd_vel.linear.x; + double wz = cmd_vel.angular.z; + ref_left = (vx - wz * 0.45 / 2) / 0.075 *180 / 3.1415926; + ref_right = (vx + wz * 0.45 / 2) / 0.075 *180 / 3.1415926; +} +ros::Subscriber<geometry_msgs::Twist> cmd("/cmd_vel", get_cmd_vel ); + +// Define Motor Driver Functions +float saturation(float cmd){ + float sig = cmd; + if(sig >= 1){ + sig = 1; + } + else if(sig <= -1){ + sig = -1; + } + return sig; +} + +void motor_drive_left( float pwm_cmd ){ + if (pwm_cmd >= 0){ + motor_left_blue.write(pwm_cmd); + motor_left_green.write(0); + } + else{ + motor_left_blue.write(0); + motor_left_green.write(-pwm_cmd); + } + +} +void motor_drive_right( float pwm_cmd ){ + if (pwm_cmd >= 0){ + motor_right_blue.write(pwm_cmd); + motor_right_green.write(0); + } + else{ + motor_right_blue.write(0); + motor_right_green.write(-pwm_cmd); + } +} + + +// Define initial setup +void init_setup(){ + // ROS initial setup + nh.initNode();// + nh.advertise(encoder_left); + nh.advertise(encoder_right); + nh.advertise(Test); + // Encoder Init + EncoderInit(&encoder2, &timer2, TIM2, 0xffffffff, TIM_ENCODERMODE_TI12); + EncoderInit(&encoder1, &timer1, TIM3, 0xffffffff, TIM_ENCODERMODE_TI12); + // Set pwm frequency + motor_left_blue.period_ms(10); // 100 Hz + motor_left_green.period_ms(10); // 100 Hz + motor_right_blue.period_ms(10); // 100 Hz + motor_right_green.period_ms(10); // 100 Hz + + nh.subscribe(cmd); +} + +int32_t count_left = 0, count_left_last = 0; +int32_t count_right = 0, count_right_last = 0; +int32_t cycle_left = 0, cycle_right = 0; +float enc_left = 0, enc_right = 0; + +void get_cycle(){ + if (count_left - count_left_last > 55000){ //BW Cycle + cycle_left -= 1; + } + else if (count_left - count_left_last < -55000){ //FW Cycle + cycle_left += 1; + } + if (count_right - count_right_last > 55000){ //BW Cycle + cycle_right -= 1; + } + else if (count_right - count_right_last < -55000){ //FW Cycle + cycle_right += 1; + } + count_left_last = count_left; + count_right_last = count_right; +} + + + + + +int main() { + // Initialization + init_setup(); + + // Define Class + Ctrl Processor_left; + Ctrl Processor_right; + + // Initial Conditions of processor + Processor_left.init(); + Processor_right.init(); + while (1) { + + // Read Encoder Count + // Count to Deg: deg = 360/960*count; + count_left = __HAL_TIM_GET_COUNTER(&timer2); + count_right = __HAL_TIM_GET_COUNTER(&timer1); + get_cycle(); + enc_left = float(count_left + 65536*cycle_left ) *360/960; + enc_right = float(count_right + 65536*cycle_right) *360/960; + deg_left.data = enc_left; + deg_right.data = enc_right; + + encoder_left.publish( °_left ); + encoder_right.publish( °_right ); +/* + motor_right_blue.write(0.1); + motor_right_green.write(0.0); + + motor_left_blue.write(0.1); + motor_left_green.write(0.0); +*/ + // Get reference input + Processor_left.ref = ref_left; + Processor_right.ref = ref_right; + + // Calculate volt command: duty = volt / 24 * 1; + float gain_left = Processor_left.PID()/24*1; + float sig_left = saturation(gain_left); + motor_drive_left(sig_left); + + float gain_right = Processor_right.PID()/24*1; + float sig_right = saturation(gain_right); + motor_drive_right(sig_right); + +// test.data = ttt; +// Test.publish(&test); + + // Measurement and Filter + Processor_left.theta = enc_left; + Processor_left.omega_processor(); + + Processor_right.theta = enc_right; + Processor_right.omega_processor(); + + nh.spinOnce(); + wait_ms(20); + } +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Wed Mar 10 03:23:54 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/odometry.cpp Wed Mar 10 03:23:54 2021 +0000 @@ -0,0 +1,45 @@ +#include "odometry.h" +#include <math.h> +Odom::Odom() { +} + +void Odom::init(){ + // Hardware Parameters + radius = 0.075; + width = 0.045; + // Odom Variables + x = 0.0; + y = 0.0; + th = 0.0; + vx = 0.0; + vy = 0.0; + vth = 0.0; + // Calculation Parameters + enc_left = 0.0; + enc_right = 0.0; + delta_distance_left = 0.0; + delta_distance_right = 0.0; +} + +void Odom::get_vel(float v, float w){ + vx = v * cos(th); + vy = v * sin(th); + vth = w; +} + +void Odom::get_distance(float new_enc_left, float new_enc_right){ + float delta_enc_left = new_enc_left - enc_left; + delta_distance_left = delta_enc_left * radius; + enc_right = new_enc_left; + + float delta_enc_right = new_enc_right - enc_right; + delta_distance_right = delta_enc_right * radius; + enc_right = new_enc_right; +} + +void Odom::odom_calculation(){ + float delta_distance_center = ( delta_distance_right - delta_distance_left ) / 2; + x += delta_distance_center * cos(th); + y += delta_distance_center * sin(th); + th += ( delta_distance_right - delta_distance_left ) / width; +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/odometry.h Wed Mar 10 03:23:54 2021 +0000 @@ -0,0 +1,29 @@ +class Odom{ + public: + + Odom(); + // Hardware Parameters + float radius; + float width; + + // pose & velocity + float x; + float y; + float th; + float vx; + float vy; + float vth; + + // Calculation Parameters + float enc_left; + float enc_right; + float delta_distance_left; + float delta_distance_right; + + // functions + void init(); + void get_vel(float v, float w); + void get_distance(float new_enc_left, float new_enc_right); + void odom_calculation(); + +};
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ros_lib_kinetic.lib Wed Mar 10 03:23:54 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/r08522622/code/ros_lib_kinetic_1/#2a6ba9a41f70