Yulin Chen
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