주 형진
ai_car1
Dependencies: mbed ai_car ros_lib_melodic
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
--- /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
--- /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
--- /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
--- /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
--- /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
--- /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
--- /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
--- /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
--- /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
--- /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
--- /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