Diff: main.cpp

Revision:

0:a35213e1e14e

--- /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