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", &deg_left);
+ros::Publisher encoder_right("/Encoder_right", &deg_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( &deg_left );
+        encoder_right.publish( &deg_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