주 형진 / Mbed 2 deprecated ai_car1

ai_car1

Dependencies:   mbed ai_car ros_lib_melodic

main.cpp@0:a35213e1e14e, 2021-05-03 (annotated)

Committer:
wngudwls000
Date:
Mon May 03 07:22:52 2021 +0000
Revision:
0:a35213e1e14e
45

Who changed what in which revision?

UserRevisionLine numberNew contents of line
wngudwls000 0:a35213e1e14e 1 #include "mbed.h"
wngudwls000 0:a35213e1e14e 2 #include "QEncoder.h"
wngudwls000 0:a35213e1e14e 3 #include "SpeedController.h"
wngudwls000 0:a35213e1e14e 4 #include "MPU9250_SPI.h"
wngudwls000 0:a35213e1e14e 5 #include "Motor.h"
wngudwls000 0:a35213e1e14e 6 #include "Servo.h"
wngudwls000 0:a35213e1e14e 7 #include <ros.h>
wngudwls000 0:a35213e1e14e 8 #include <std_msgs/Int32.h>
wngudwls000 0:a35213e1e14e 9 #include <std_msgs/Float32.h>
wngudwls000 0:a35213e1e14e 10 #include <std_msgs/Float32MultiArray.h>
wngudwls000 0:a35213e1e14e 11 #define PI 3.141592653589793
wngudwls000 0:a35213e1e14e 12 MPU9250_SPI mpu(p5,p6,p7,p8,p12); //mosi miso sclk cs intr
wngudwls000 0:a35213e1e14e 13 Servo servo(p13);
wngudwls000 0:a35213e1e14e 14 QEncoder enco(p15, p16);
wngudwls000 0:a35213e1e14e 15 Motor motor (p23, p24, p25, p26);
wngudwls000 0:a35213e1e14e 16 DigitalOut led1 = LED1;
wngudwls000 0:a35213e1e14e 17 PIDController speedController(0.085, 0.245, 0.00025, 100.0, 0.0, -1.0, 100.0);//0.05514
wngudwls000 0:a35213e1e14e 18
wngudwls000 0:a35213e1e14e 19 Timer timer;
wngudwls000 0:a35213e1e14e 20 Timer tmr;
wngudwls000 0:a35213e1e14e 21
wngudwls000 0:a35213e1e14e 22 ros::NodeHandle nh;
wngudwls000 0:a35213e1e14e 23 std_msgs::Int32 msg_encoder;
wngudwls000 0:a35213e1e14e 24 std_msgs::Float32MultiArray msg_imu_raw;
wngudwls000 0:a35213e1e14e 25 std_msgs::Float32MultiArray msg_imu_mag;
wngudwls000 0:a35213e1e14e 26
wngudwls000 0:a35213e1e14e 27 unsigned int uiFlag_1ms = 0;
wngudwls000 0:a35213e1e14e 28 float PWM = 0;
wngudwls000 0:a35213e1e14e 29 float dt;
wngudwls000 0:a35213e1e14e 30
wngudwls000 0:a35213e1e14e 31 void getDt(){
wngudwls000 0:a35213e1e14e 32 dt=tmr.read_us()/1000000.0;
wngudwls000 0:a35213e1e14e 33 tmr.reset();
wngudwls000 0:a35213e1e14e 34 }
wngudwls000 0:a35213e1e14e 35
wngudwls000 0:a35213e1e14e 36 void applyCalbratedValue(){
wngudwls000 0:a35213e1e14e 37 Vect3 gBias ={ 0.236 , 1.527 , -0.558};
wngudwls000 0:a35213e1e14e 38 mpu.setGyroBias(gBias);
wngudwls000 0:a35213e1e14e 39 Vect3 mBias ={-393.590, 334.364,368.100};
wngudwls000 0:a35213e1e14e 40 mpu.setMagBias(mBias);
wngudwls000 0:a35213e1e14e 41 Vect3 mScale={ 1.058, 1.022 , 0.929};
wngudwls000 0:a35213e1e14e 42 mpu.setMagScale(mScale);
wngudwls000 0:a35213e1e14e 43 Vect3 aBias = { -0.095, 0.03 , -0.10};
wngudwls000 0:a35213e1e14e 44 mpu.setAccBias(aBias);
wngudwls000 0:a35213e1e14e 45 }
wngudwls000 0:a35213e1e14e 46
wngudwls000 0:a35213e1e14e 47 void servo_cb(const std_msgs::Int32& cmd_msg)
wngudwls000 0:a35213e1e14e 48 {
wngudwls000 0:a35213e1e14e 49 servo.update(cmd_msg.data);
wngudwls000 0:a35213e1e14e 50 }
wngudwls000 0:a35213e1e14e 51 void motor_cb(const std_msgs::Float32& cmd_msg)
wngudwls000 0:a35213e1e14e 52 {
wngudwls000 0:a35213e1e14e 53 PWM = cmd_msg.data;
wngudwls000 0:a35213e1e14e 54 }
wngudwls000 0:a35213e1e14e 55
wngudwls000 0:a35213e1e14e 56 ros::Subscriber<std_msgs::Int32> servo_sub("servo", &servo_cb);
wngudwls000 0:a35213e1e14e 57 ros::Subscriber<std_msgs::Float32> motor_sub("motor", &motor_cb);
wngudwls000 0:a35213e1e14e 58 ros::Publisher encoder_pub("encoder", &msg_encoder);
wngudwls000 0:a35213e1e14e 59 ros::Publisher imu_raw_pub("imu_raw", &msg_imu_raw);
wngudwls000 0:a35213e1e14e 60 ros::Publisher imu_mag_pub("imu_mag", &msg_imu_mag);
wngudwls000 0:a35213e1e14e 61
wngudwls000 0:a35213e1e14e 62 void counter_1ms ()
wngudwls000 0:a35213e1e14e 63 {
wngudwls000 0:a35213e1e14e 64 uiFlag_1ms++;
wngudwls000 0:a35213e1e14e 65 }
wngudwls000 0:a35213e1e14e 66
wngudwls000 0:a35213e1e14e 67 void pub_sub_setup()
wngudwls000 0:a35213e1e14e 68 {
wngudwls000 0:a35213e1e14e 69 nh.initNode();
wngudwls000 0:a35213e1e14e 70 nh.advertise(encoder_pub);
wngudwls000 0:a35213e1e14e 71 nh.advertise(imu_raw_pub);
wngudwls000 0:a35213e1e14e 72 nh.advertise(imu_mag_pub);
wngudwls000 0:a35213e1e14e 73 nh.subscribe(servo_sub);
wngudwls000 0:a35213e1e14e 74 nh.subscribe(motor_sub);
wngudwls000 0:a35213e1e14e 75 }
wngudwls000 0:a35213e1e14e 76
wngudwls000 0:a35213e1e14e 77 int main() {
wngudwls000 0:a35213e1e14e 78 enco.init();
wngudwls000 0:a35213e1e14e 79 enco.setCount(0);
wngudwls000 0:a35213e1e14e 80
wngudwls000 0:a35213e1e14e 81 mpu.setup();
wngudwls000 0:a35213e1e14e 82 mpu.setMagneticDeclination(8.5);
wngudwls000 0:a35213e1e14e 83 mpu.setSampleRate(SR_100HZ);
wngudwls000 0:a35213e1e14e 84 mpu.setGyroRange(GYRO_RANGE_2000DPS);
wngudwls000 0:a35213e1e14e 85 mpu.setAccelRange(ACCEL_RANGE_16G);
wngudwls000 0:a35213e1e14e 86 mpu.setDlpfBandwidth(DLPF_BANDWIDTH_184HZ);
wngudwls000 0:a35213e1e14e 87 mpu.enableDataReadyInterrupt();
wngudwls000 0:a35213e1e14e 88 applyCalbratedValue();
wngudwls000 0:a35213e1e14e 89 tmr.start(); tmr.reset();
wngudwls000 0:a35213e1e14e 90
wngudwls000 0:a35213e1e14e 91 pub_sub_setup();
wngudwls000 0:a35213e1e14e 92
wngudwls000 0:a35213e1e14e 93 Ticker ticker_1ms;
wngudwls000 0:a35213e1e14e 94 ticker_1ms.attach(&counter_1ms, 0.001);
wngudwls000 0:a35213e1e14e 95 timer.start();
wngudwls000 0:a35213e1e14e 96
wngudwls000 0:a35213e1e14e 97 float degree = 0.0;
wngudwls000 0:a35213e1e14e 98 servo.update (degree);
wngudwls000 0:a35213e1e14e 99
wngudwls000 0:a35213e1e14e 100 int count = 0;
wngudwls000 0:a35213e1e14e 101 int RPM = 0;
wngudwls000 0:a35213e1e14e 102
wngudwls000 0:a35213e1e14e 103
wngudwls000 0:a35213e1e14e 104 float curr_speed = 0.0;
wngudwls000 0:a35213e1e14e 105 float u_percent= 0.0;
wngudwls000 0:a35213e1e14e 106
wngudwls000 0:a35213e1e14e 107 float raw_parm[6];
wngudwls000 0:a35213e1e14e 108 float mag_parm[3];
wngudwls000 0:a35213e1e14e 109 while(1) {
wngudwls000 0:a35213e1e14e 110 // Every 1 ms,
wngudwls000 0:a35213e1e14e 111 if(uiFlag_1ms >= 1) {
wngudwls000 0:a35213e1e14e 112 uiFlag_1ms = 0;
wngudwls000 0:a35213e1e14e 113
wngudwls000 0:a35213e1e14e 114 // Speed control
wngudwls000 0:a35213e1e14e 115 speedController.setTarget(PWM);
wngudwls000 0:a35213e1e14e 116 curr_speed = RPM * 2 * PI * 0.045 / 60;
wngudwls000 0:a35213e1e14e 117 u_percent = speedController.update (curr_speed, timer.read());
wngudwls000 0:a35213e1e14e 118 motor.setSpeed_percent(u_percent, FORWARD);
wngudwls000 0:a35213e1e14e 119 }
wngudwls000 0:a35213e1e14e 120 if(mpu.isDataReady())
wngudwls000 0:a35213e1e14e 121 {
wngudwls000 0:a35213e1e14e 122 getDt();
wngudwls000 0:a35213e1e14e 123 Vect3 a, g, m; // acc/gyro/mag vectors
wngudwls000 0:a35213e1e14e 124 mpu.update(a,g,m);
wngudwls000 0:a35213e1e14e 125 msg_imu_raw.data = raw_parm;
wngudwls000 0:a35213e1e14e 126 msg_imu_mag.data = mag_parm;
wngudwls000 0:a35213e1e14e 127 msg_imu_raw.data_length = 6;
wngudwls000 0:a35213e1e14e 128 msg_imu_mag.data_length = 3;
wngudwls000 0:a35213e1e14e 129 msg_imu_raw.data[0] = a.x;
wngudwls000 0:a35213e1e14e 130 msg_imu_raw.data[1] = a.y;
wngudwls000 0:a35213e1e14e 131 msg_imu_raw.data[2] = a.z;
wngudwls000 0:a35213e1e14e 132 msg_imu_raw.data[3] = g.x;
wngudwls000 0:a35213e1e14e 133 msg_imu_raw.data[4] = g.y;
wngudwls000 0:a35213e1e14e 134 msg_imu_raw.data[5] = g.z;
wngudwls000 0:a35213e1e14e 135 msg_imu_mag.data[0] = m.x;
wngudwls000 0:a35213e1e14e 136 msg_imu_mag.data[1] = m.y;
wngudwls000 0:a35213e1e14e 137 msg_imu_mag.data[2] = m.z;
wngudwls000 0:a35213e1e14e 138 imu_raw_pub.publish(&msg_imu_raw);
wngudwls000 0:a35213e1e14e 139 imu_mag_pub.publish(&msg_imu_mag);
wngudwls000 0:a35213e1e14e 140 }
wngudwls000 0:a35213e1e14e 141
wngudwls000 0:a35213e1e14e 142 led1 = !led1;
wngudwls000 0:a35213e1e14e 143
wngudwls000 0:a35213e1e14e 144 count = enco.getCount();
wngudwls000 0:a35213e1e14e 145 RPM = -(60 * count) / (timer.read() * 3 * 200 * 4);
wngudwls000 0:a35213e1e14e 146
wngudwls000 0:a35213e1e14e 147 msg_encoder.data = RPM;
wngudwls000 0:a35213e1e14e 148 encoder_pub.publish(&msg_encoder);
wngudwls000 0:a35213e1e14e 149
wngudwls000 0:a35213e1e14e 150 timer.reset();
wngudwls000 0:a35213e1e14e 151 enco.setCount(0);
wngudwls000 0:a35213e1e14e 152
wngudwls000 0:a35213e1e14e 153 nh.spinOnce();
wngudwls000 0:a35213e1e14e 154 wait(0.1);
wngudwls000 0:a35213e1e14e 155 }
wngudwls000 0:a35213e1e14e 156 }