Revision 0:a35213e1e14e, committed 2021-05-03

Comitter:

wngudwls000

Date:

Mon May 03 07:22:52 2021 +0000

Commit message:

45

Changed in this revision

diff -r 000000000000 -r a35213e1e14e Actuator/Motor.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Actuator/Motor.cpp	Mon May 03 07:22:52 2021 +0000
@@ -0,0 +1,41 @@
+#include "Motor.h"
+
+
+Motor::Motor(PinName IN1, PinName IN2, PinName INH1, PinName INH2)
+    : IN1_(IN1), IN2_(IN2), INH1_(INH1), INH2_(INH2)
+{
+    INH1_=1;
+    INH2_=1;
+    IN1_.period_us(50);
+    IN2_.period_us(50);
+}
+
+void Motor::setSpeed_percent(float percent, char direction)
+{
+    float duty = percent/100.0;
+    if(duty>1.0) {
+        duty=1.0;
+    } else if(duty<0.0) {
+        duty=0.0;
+    }
+    
+    if(direction==FORWARD) 
+    {
+        IN1_=duty;
+        IN2_=0;
+    } 
+    else if (direction == BACKWARD){
+        IN1_=0;
+        IN2_=duty;
+    }
+    else if (direction == BREAK)
+    {
+        IN1_=1;
+        IN2_=1;
+    }
+    else 
+    {
+        IN1_=0;
+        IN2_=0;
+    }   
+}
\ No newline at end of file

diff -r 000000000000 -r a35213e1e14e Actuator/Motor.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Actuator/Motor.h	Mon May 03 07:22:52 2021 +0000
@@ -0,0 +1,22 @@
+#ifndef MBED_MOTOR_H
+#define MBED_MOTOR_H
+
+#include "mbed.h"
+
+#define FORWARD 'f'
+#define BACKWARD 'b'
+#define BREAK 's'
+
+class Motor
+{
+private:
+    PwmOut IN1_, IN2_;
+    DigitalOut INH1_, INH2_;
+
+public:
+    Motor(PinName IN1, PinName IN2,PinName INH1, PinName INH2);
+
+    void setSpeed_percent(float percent, char direction);
+};
+
+#endif

diff -r 000000000000 -r a35213e1e14e Actuator/Servo.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Actuator/Servo.cpp	Mon May 03 07:22:52 2021 +0000
@@ -0,0 +1,39 @@
+#include "Servo.h"
+
+Servo::Servo(PinName IN): m_PWM(IN)
+{
+    m_degree=0;
+    m_width= MID+(0.0005/90.0)*m_degree;
+    m_period_ticker.attach(callback(this, &Servo::setPeriod),0.02);
+    m_width_timeout.attach(callback(this, &Servo::setWidth),m_width);
+    m_PWM=1;
+}
+
+void Servo::setWidth()
+{
+    m_PWM=0;
+}
+
+void Servo::setPeriod()
+{
+    m_width_timeout.attach(callback(this, &Servo::setWidth),m_width);
+    m_PWM=1;
+}
+
+float Servo::getDegree()
+{
+    return m_degree;
+}
+
+void Servo::update(float degree)
+{
+    if(degree>MAX) {
+        degree=MAX;
+    }
+    if(degree<-1*MAX) {
+        degree=MAX*-1;
+    }
+
+    m_degree=degree;
+    m_width= MID+(0.0005/90.0)*degree;
+}
\ No newline at end of file

diff -r 000000000000 -r a35213e1e14e Actuator/Servo.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Actuator/Servo.h	Mon May 03 07:22:52 2021 +0000
@@ -0,0 +1,26 @@
+#ifndef MBED_SERVO_H
+#define MBED_SERVO_H
+
+#include "mbed.h"
+
+#define MAX 75
+#define MID 0.00165
+
+class Servo
+{
+private:
+    DigitalOut m_PWM;
+    Ticker m_period_ticker;
+    Timeout m_width_timeout;
+    float m_degree;
+    float m_width;
+    
+    void setPeriod();
+    void setWidth();
+public:
+    Servo(PinName IN);
+    void update(float degree);
+    float getDegree();
+};
+
+#endif
\ No newline at end of file

diff -r 000000000000 -r a35213e1e14e Controller/SpeedController.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Controller/SpeedController.cpp	Mon May 03 07:22:52 2021 +0000
@@ -0,0 +1,95 @@
+#include "SpeedController.h"
+
+
+PIDController::PIDController(float kp, float ki, float kd, float max_windup,
+                             float start_time, float umin, float umax)
+{
+//    The PID controller can be initalized with a specific kp value
+//    ki value, and kd value
+    this->kp = kp;
+    this->ki = ki;
+    this->kd = kd;
+
+    this->max_windup = max_windup;
+
+    this->umin = umin;
+    this->umax = umax;
+
+    //Store relevant data
+    this->m_last_timestamp = 0.0;
+    this->m_set_point = 0.0;
+    this->m_start_time = start_time;
+    this->m_error_sum = 0.0;
+    this->m_last_error = 0.0;
+}
+
+
+
+float PIDController::update(float measured_value, float timestamp)
+{   
+    float delta_time = timestamp - m_last_timestamp;
+    float error = m_set_point - measured_value;
+    
+    m_last_timestamp = timestamp;
+    
+    
+    m_error_sum += error * delta_time;
+    
+    float delta_error = error - m_last_error;
+    
+    if(delta_error > 10.0) {
+        delta_error = 0.0;
+    }
+    
+    m_last_error = error;
+
+    if(m_error_sum > max_windup) {
+        m_error_sum = max_windup;
+    } else if(m_error_sum < -1.0*max_windup) {
+        m_error_sum = max_windup*-1.0;
+    }
+
+    float p = kp * error;
+    float i = ki * m_error_sum;
+    float d = kd * (delta_error/delta_time);
+
+    float u=p+i+d;
+
+    if(u > umax) {
+        u = umax;
+    } else if(u < umin) {
+        u = umin;
+    }
+
+    return u;
+}
+
+void PIDController::setTarget(float target)
+{
+    this->m_set_point = target;
+}
+
+void PIDController::setKp(float kp)
+{
+    this->kp = kp;
+}
+
+void PIDController::setKi(float ki)
+{
+    this->ki = ki;
+}
+
+void PIDController::setKd(float kd)
+{
+    this->kd = kd;
+}
+
+void PIDController::setMaxWindup(float max_windup)
+{
+    this->max_windup = max_windup;
+}
+
+float PIDController::getLastTimeStamp()
+{
+    return this->m_last_timestamp;
+}
\ No newline at end of file

diff -r 000000000000 -r a35213e1e14e Controller/SpeedController.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Controller/SpeedController.h	Mon May 03 07:22:52 2021 +0000
@@ -0,0 +1,37 @@
+#ifndef MBED_SPEEDCONTROLLER_H
+#define MBED_SPEEDCONTROLLER_H
+
+#include "mbed.h"
+
+
+class PIDController
+{
+private:
+    float kp;
+    float ki;
+    float kd;
+    float max_windup;
+    float umin;
+    float umax;
+
+    float m_last_timestamp;
+    float m_set_point;
+    float m_start_time;
+    float m_error_sum;
+    float m_last_error;
+    float m_last_d;
+
+public:
+    PIDController(float kp, float ki, float kd, float max_windup,
+                             float start_time,float umin, float umax);
+    //void reset();
+    float update(float measured_value, float timestamp);
+    void setTarget(float target);
+    void setKp(float kp);
+    void setKi(float ki);
+    void setKd(float kd);
+    void setMaxWindup(float max_windup);
+    float getLastTimeStamp();
+};
+
+#endif

diff -r 000000000000 -r a35213e1e14e MPU9250_SPI.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU9250_SPI.lib	Mon May 03 07:22:52 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/wngudwls000/code/ai_car/#f3816a0f498e

diff -r 000000000000 -r a35213e1e14e QEncoder/QEncoder.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEncoder/QEncoder.h	Mon May 03 07:22:52 2021 +0000
@@ -0,0 +1,59 @@
+/* Qencoder Version 1.0 */
+#pragma once  //instead of safe include guard
+#include "mbed.h"
+
+extern DigitalOut led1;
+
+class QEncoder {
+    public:
+    QEncoder(PinName pinA, PinName pinB)
+      : _pinA(pinA), _pinB(pinB),_bi(pinA,pinB){}
+    void init()    {
+        _pinA.mode(PullUp);
+        _pinB.mode(PullUp);
+        _pinA.rise(callback(this, &QEncoder::decode)); // callback for edges
+        _pinA.fall(callback(this, &QEncoder::decode));
+        _pinB.rise(callback(this, &QEncoder::decode));
+        _pinB.fall(callback(this, &QEncoder::decode));
+        _previousState = _bi;
+        _count = 0;
+        _errorCount=0;
+    }
+    int32_t getCount()    {
+        return _count;
+    }
+    void setCount(int32_t EncoderVal){
+        _pinA.disable_irq (); _pinB.disable_irq ();
+        _count = EncoderVal;
+        _pinA.enable_irq ();_pinB.enable_irq ();
+    }
+    int32_t getErrorCount()    {
+        return _errorCount;
+    }
+    
+  private:
+    InterruptIn _pinA;
+    InterruptIn _pinB;
+    BusIn _bi;
+    uint8_t _previousState;
+    volatile int32_t _count;
+    volatile uint32_t _errorCount;
+    void decode()    {
+        uint8_t newState = _bi;
+        switch((_previousState << 2) | newState)         {
+        case 0b0001: // 0x01 PREV|NEW
+        case 0b0111: // 0x07
+        case 0b1110: // 0x0E
+        case 0b1000: // 0x08
+            _count --; break;
+        case 0b0010: // 0x02
+        case 0b1011: // 0x0B
+        case 0b1101: // 0x0D
+        case 0b0100: // 0x04
+            _count ++; break;
+        default:
+            _errorCount++; break;
+        } 
+        _previousState = newState; 
+    } 
+};
\ No newline at end of file

diff -r 000000000000 -r a35213e1e14e main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon May 03 07:22:52 2021 +0000
@@ -0,0 +1,156 @@
+#include "mbed.h"
+#include "QEncoder.h"
+#include "SpeedController.h"
+#include "MPU9250_SPI.h"
+#include "Motor.h"
+#include "Servo.h"
+#include <ros.h>
+#include <std_msgs/Int32.h>
+#include <std_msgs/Float32.h>
+#include <std_msgs/Float32MultiArray.h> 
+#define PI 3.141592653589793
+MPU9250_SPI mpu(p5,p6,p7,p8,p12); //mosi miso sclk cs intr
+Servo servo(p13);
+QEncoder enco(p15, p16);
+Motor motor (p23, p24, p25, p26);
+DigitalOut led1 = LED1;
+PIDController speedController(0.085, 0.245, 0.00025, 100.0, 0.0, -1.0, 100.0);//0.05514
+
+Timer timer;
+Timer tmr;
+
+ros::NodeHandle nh;
+std_msgs::Int32 msg_encoder;
+std_msgs::Float32MultiArray msg_imu_raw;
+std_msgs::Float32MultiArray msg_imu_mag;
+
+unsigned int uiFlag_1ms = 0;
+float PWM = 0;
+float dt;
+
+void getDt(){
+  dt=tmr.read_us()/1000000.0;
+  tmr.reset();
+}
+
+void applyCalbratedValue(){
+  Vect3 gBias ={ 0.236 , 1.527  ,  -0.558};
+  mpu.setGyroBias(gBias); 
+  Vect3 mBias ={-393.590,  334.364,368.100};
+  mpu.setMagBias(mBias);
+  Vect3 mScale={ 1.058,    1.022 , 0.929}; 
+  mpu.setMagScale(mScale); 
+  Vect3 aBias = { -0.095, 0.03 , -0.10};
+  mpu.setAccBias(aBias);
+}
+
+void servo_cb(const std_msgs::Int32& cmd_msg)
+{
+    servo.update(cmd_msg.data);
+}
+void motor_cb(const std_msgs::Float32& cmd_msg)
+{
+    PWM = cmd_msg.data;
+}
+
+ros::Subscriber<std_msgs::Int32> servo_sub("servo", &servo_cb);
+ros::Subscriber<std_msgs::Float32> motor_sub("motor", &motor_cb);
+ros::Publisher encoder_pub("encoder", &msg_encoder);
+ros::Publisher imu_raw_pub("imu_raw", &msg_imu_raw);
+ros::Publisher imu_mag_pub("imu_mag", &msg_imu_mag);
+
+void counter_1ms ()
+{
+    uiFlag_1ms++;
+}
+
+void pub_sub_setup()
+{
+    nh.initNode();
+    nh.advertise(encoder_pub);
+    nh.advertise(imu_raw_pub);
+    nh.advertise(imu_mag_pub);
+    nh.subscribe(servo_sub);
+    nh.subscribe(motor_sub);
+}
+
+int main() {
+    enco.init();
+    enco.setCount(0);
+    
+    mpu.setup();
+    mpu.setMagneticDeclination(8.5);
+    mpu.setSampleRate(SR_100HZ);
+    mpu.setGyroRange(GYRO_RANGE_2000DPS);
+    mpu.setAccelRange(ACCEL_RANGE_16G);
+    mpu.setDlpfBandwidth(DLPF_BANDWIDTH_184HZ); 
+    mpu.enableDataReadyInterrupt();
+    applyCalbratedValue();
+    tmr.start(); tmr.reset();
+
+    pub_sub_setup();
+    
+    Ticker ticker_1ms;
+    ticker_1ms.attach(&counter_1ms, 0.001);
+    timer.start();
+    
+    float degree = 0.0;
+    servo.update (degree);
+
+    int count = 0;
+    int RPM = 0;
+        
+    
+    float curr_speed = 0.0;
+    float u_percent= 0.0;
+
+    float raw_parm[6];
+    float mag_parm[3];
+    while(1) {
+        // Every 1 ms,
+        if(uiFlag_1ms >= 1) {
+            uiFlag_1ms = 0;
+        
+            // Speed control
+            speedController.setTarget(PWM);
+            curr_speed = RPM * 2 * PI * 0.045 / 60;
+            u_percent = speedController.update (curr_speed, timer.read());
+            motor.setSpeed_percent(u_percent, FORWARD);
+        }
+        if(mpu.isDataReady())
+        {
+            getDt();
+            Vect3  a, g, m;  // acc/gyro/mag vectors
+            mpu.update(a,g,m);
+            msg_imu_raw.data = raw_parm;
+            msg_imu_mag.data = mag_parm;
+            msg_imu_raw.data_length = 6;
+            msg_imu_mag.data_length = 3;
+            msg_imu_raw.data[0] = a.x;
+            msg_imu_raw.data[1] = a.y;
+            msg_imu_raw.data[2] = a.z;
+            msg_imu_raw.data[3] = g.x;
+            msg_imu_raw.data[4] = g.y;
+            msg_imu_raw.data[5] = g.z;
+            msg_imu_mag.data[0] = m.x;
+            msg_imu_mag.data[1] = m.y;
+            msg_imu_mag.data[2] = m.z;
+            imu_raw_pub.publish(&msg_imu_raw);
+            imu_mag_pub.publish(&msg_imu_mag);
+        }
+
+        led1 = !led1; 
+        
+        count = enco.getCount();
+        RPM = -(60 * count) / (timer.read() * 3 * 200 * 4);
+        
+        msg_encoder.data = RPM;
+        encoder_pub.publish(&msg_encoder);
+        
+        timer.reset();
+        enco.setCount(0);
+        
+        nh.spinOnce();
+        wait(0.1);
+    }
+}
\ No newline at end of file

diff -r 000000000000 -r a35213e1e14e mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Mon May 03 07:22:52 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file

diff -r 000000000000 -r a35213e1e14e ros_lib_melodic.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ros_lib_melodic.lib	Mon May 03 07:22:52 2021 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/garyservin/code/ros_lib_melodic/#da82487f547e