Dragon
code including all sensors
Diff: main.cpp
- Revision:
- 0:66a265dc3146
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Mon Nov 25 10:51:18 2019 +0000
@@ -0,0 +1,632 @@
+/* mbed Microcontroller Library
+ * Copyright (c) 2018 ARM Limited
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "mbed.h"
+#include "ak8963.h"
+#include <stddef.h>
+#include <stdio.h> // This ert_main.c example uses printf/fflush
+#include "LAAP.h" // Model's header file
+#include "rtwtypes.h"
+
+static LAAPModelClass rtObj; // Instance of model class
+
+void rt_OneStep(void);
+void rt_OneStep(void)
+{
+ static boolean_T OverrunFlag = false;
+
+ // Disable interrupts here
+
+ // Check for overrun
+ if (OverrunFlag) {
+ rtmSetErrorStatus(rtObj.getRTM(), "Overrun");
+ return;
+ }
+
+ OverrunFlag = true;
+
+ // Save FPU context here (if necessary)
+ // Re-enable timer or interrupt here
+ // Set model inputs here
+
+ // Step the model
+ rtObj.step();
+
+ // Get model outputs here
+
+ // Indicate task complete
+ OverrunFlag = false;
+
+ // Disable interrupts here
+ // Restore FPU context here (if necessary)
+ // Enable interrupts here
+}
+/*
+#define AK8963_ADDRESS 0x0C
+#define WHO_AM_I_AK8963 0x00 // should return 0x48
+#define INFO 0x01
+#define AK8963_ST1 0x02 // data ready status bit 0
+#define AK8963_XOUT_L 0x03 // data
+#define AK8963_XOUT_H 0x04
+#define AK8963_YOUT_L 0x05
+#define AK8963_YOUT_H 0x06
+#define AK8963_ZOUT_L 0x07
+#define AK8963_ZOUT_H 0x08
+#define AK8963_ST2 0x09 // Data overflow bit 3 and data read error status bit 2
+#define AK8963_CNTL 0x0A // Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0
+#define AK8963_ASTC 0x0C // Self test control
+#define AK8963_I2CDIS 0x0F // I2C disable
+#define AK8963_ASAX 0x10 // Fuse ROM x-axis sensitivity adjustment value
+#define AK8963_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value
+#define AK8963_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value
+*/
+#define USER_CTRL 0x6A;
+#define I2C_MST_EN 0x20;
+
+#define MPU6050_ADDR 0xD0
+#define MPU6050_SMPLRT_DIV 0x19
+#define MPU6050_CONFIG 0x1a
+#define MPU6050_GYRO_CONFIG 0x1b
+#define MPU6050_ACCEL_CONFIG 0x1c
+#define MPU6050_WHO_AM_I 0x75
+#define MPU6050_PWR_MGMT_1 0x6b
+#define ACCEL_XOUT_H_REG 0x3B
+#define Alfa_comp 0.96
+
+ double Ax, Ay, Az, Gx, Gy, Gz,Mx,My,Mz;
+
+ double H_senx,H_seny,H_senz;
+ char name[1];
+
+ int16_t Mag_X_RAW = 0;
+ int16_t Mag_Y_RAW = 0;
+ int16_t Mag_Z_RAW = 0;
+
+ double avgx,avgy,avgz;
+ double rate,bias=0;
+ double angle;
+
+ double pitch_acc=0, roll_acc=0;
+ double pitch_gyro=0, roll_gyro=0;
+ double pitch=0, roll=0,yaw=0;
+ double dtx,dtx1,dtx2,dt=0,dt1,dt2;
+ //global variables for meaured pwm values
+ uint16_t Rip_chnl_n[6],Rip_chnl_p[6];
+
+ int distx,disty,distz;
+ int distxo,distyo,distzo;
+ int velocityx,velocityy,velocityz;
+
+//////////////////////To measure pwm using interrupts and timer///////////////////////////////
+InterruptIn ch1(PG_2);
+InterruptIn ch2(PG_3);
+InterruptIn ch3(A4);
+InterruptIn ch4(A5);
+InterruptIn ch5(PG_0);
+InterruptIn ch6(PG_1);
+///////////////////////////////////////////////////////////////////////////////////////////
+//////////////////// pwm out for ESC inputs////////////////////////////////////////////////
+PwmOut u1(PE_9);
+PwmOut u2(PE_11);
+PwmOut u3(PE_12);
+PwmOut u4(PE_14);
+///////////////////////////I2C/////////////////////////////////////////////////////////////
+I2C i2c(PF_15,PF_14);
+//////////////////////////////////////////////////////////////////////////////////////////
+///////////////////////////////LIDAR serial ports/////////////////////////////////////////
+RawSerial rawz(PD_5,PD_6);
+RawSerial rawx(PC_10,PC_11);
+RawSerial rawy(PC_12,PD_2);
+//////////////////////////////////////////////////////////////////////////////////////////
+//////////blue light indicator for error in i2c///////////////////////////////////////////
+DigitalOut myled(LED2);
+//////////////////////////////////////////////////////////////////////////////////////////
+Serial pc(SERIAL_TX, SERIAL_RX);
+
+Timeout onestep;
+
+Timer timer1,timer2;
+////////////////////////////////function initializations/////////////////////////////////
+void stop_counter();
+void start_counter();
+void counters_init();
+
+void dist_x();
+void dist_y();
+void dist_z();
+
+void mpu_init();
+void mpu_read();
+void get_angle();
+void mpu_calibrate();
+void Mag_init();
+void Mag_read();
+
+onestep.attach_us(&rt_OneStep(),100);
+
+int main()
+{
+ counters_init();
+
+ pc.baud(115200);
+
+ rawx.baud(115200);
+ rawy.baud(115200);
+ rawz.baud(115200);
+
+ rawx.attach(&dist_x);
+ rawy.attach(&dist_y);
+ rawz.attach(&dist_z);
+
+ mpu_init();
+ //Mag_init();
+ /*AK8963 mag(&i2c, AK8963::SLAVE_ADDR_1);
+ if(mag.checkConnection() != AK8963::SUCCESS){
+ pc.printf("check connection");
+
+ }
+ if(mag.setOperationMode(AK8963::MODE_CONTINUOUS_1) != AK8963::SUCCESS){
+ while(1){pc.printf("failed continious mode");}
+ }*/
+ timer1.start(); //////////////timer for channel
+ timer2.start(); /////////////timer for dt to calculate dt
+
+ //mpu_calibrate();
+
+
+ u1.period_us(20000);
+ u2.period_us(20000);
+ u3.period_us(20000);
+ u4.period_us(20000);
+
+
+ while(timer2.read_ms()<5000)
+ { u1.write((float)rtObj.getu1_());
+ u2.write((float)rtObj.getu2_());
+ u3.write((float)rtObj.getu3 ());
+ u4.write((float)rtObj.getu4 ());
+
+ }
+
+
+
+
+ while (true)
+ { int i;
+ //int statusAK8963= AK8963::NOT_DATA_READY;
+
+
+ get_angle();
+
+ /* if(statusAK8963 == AK8963::DATA_READY){
+ AK8963::MagneticVector mag1;
+ mag.getMagneticVector(&mag1);
+ pc.printf("%5.1f,%5.1f,%5.1f\n",mag1.mx,mag1.my,mag1.mz);
+ statusAK8963 = AK8963::NOT_DATA_READY;
+ } else if (statusAK8963 == AK8963::NOT_DATA_READY){
+ }
+
+
+ for(int i=1000;i<2000;i++){
+ u1.write((float)i/20000.0);
+ u2.write((float)i/20000.0);
+ u3.write((float)i/20000.0);
+ u4.write((float)i/20000.0);
+ wait(0.0001);}
+ for(int i=2000;i>1000;i--){
+ u1.write((float)i/20000.0);
+ u2.write((float)i/20000.0);
+ u3.write((float)i/20000.0);
+ u4.write((float)i/20000.0);
+ wait(0.0001);}*/
+ // Mag_read();
+ //for(i=0;i<6;i++)
+ //pc.printf("%d\t",Rip_chnl_n[i]);
+ // pc.printf("\n");
+ /* pc.printf("pitch = %f\t",pitch);
+ pc.printf("roll = %f\t",roll);
+ pc.printf("yaw = %f\t",yaw);
+ */
+ pc.printf("x = %d\t",distx);
+ pc.printf("y = %d\t",disty);
+ pc.printf("z = %d\n",distz);
+
+
+
+ //pc.printf("magx = %d\t",name[0]);
+ //pc.printf("raw = %d\t",Mag_X_RAW);
+ //pc.printf("mx = %f\t",Mx);
+ //wait(0.01);
+ //
+ }
+}
+
+
+///////////////////////////////////////////////////////////////functions//////////////////////////////////////////////////////////
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+//////////////////////////////********************************functions for interrupts*****************************////////////////
+void start_counter_1()
+{
+ Rip_chnl_p[0]=timer1.read_us();
+ }
+void start_counter_2(){
+ Rip_chnl_p[1]=timer1.read_us();
+ }
+void start_counter_3(){
+ Rip_chnl_p[2]=timer1.read_us();
+ }
+void start_counter_4(){
+ Rip_chnl_p[3]=timer1.read_us();
+ }
+void start_counter_5(){
+ Rip_chnl_p[4]=timer1.read_us();
+ }
+void start_counter_6(){
+ Rip_chnl_p[5]=timer1.read_us();
+ }
+void stop_counter_1(){
+ uint32_t x;
+ x=timer1.read_us();
+ Rip_chnl_n[0]=x-Rip_chnl_p[0];
+ }
+void stop_counter_2(){
+ uint32_t x;
+ x=timer1.read_us();
+ Rip_chnl_n[1]=x-Rip_chnl_p[1];
+ }
+void stop_counter_3(){
+ uint32_t x;
+ x=timer1.read_us();
+ Rip_chnl_n[2]=x-Rip_chnl_p[2];
+ }
+void stop_counter_4(){
+ uint32_t x;
+ x=timer1.read_us();
+ Rip_chnl_n[3]=x-Rip_chnl_p[3];
+ }
+void stop_counter_5(){
+ uint32_t x;
+ x=timer1.read_us();
+ Rip_chnl_n[4]=x-Rip_chnl_p[4];
+ }
+void stop_counter_6(){
+ uint32_t x;
+ x=timer1.read_us();
+ Rip_chnl_n[5]=x-Rip_chnl_p[5];
+ }
+
+void counters_init(){
+ ch1.rise(&start_counter_1);
+ ch2.rise(&start_counter_2);
+ ch3.rise(&start_counter_3);
+ ch4.rise(&start_counter_4);
+ ch5.rise(&start_counter_5);
+ ch6.rise(&start_counter_6);
+ ch1.fall(&stop_counter_1);
+ ch2.fall(&stop_counter_2);
+ ch3.fall(&stop_counter_3);
+ ch4.fall(&stop_counter_4);
+ ch5.fall(&stop_counter_5);
+ ch6.fall(&stop_counter_6);
+ }
+
+
+//////////////////////////////***********************************LIDAR********************************************////////////////
+void dist_x() ///////////*******use this function to get distance it has object of rawserial as argument*******////////////
+ { //int distance;//actual distance measurements of LiDAR
+ int strength;//signal strength of LiDAR
+ int check;//save check value
+ int i;
+ int uartx[9];//save data measured by LiDAR
+
+ if (rawx.readable())//check if serial port has data input
+ {
+ if(rawx.getc()==0x59)//assess data package frame header 0x59
+ {
+ uartx[0]=0x59;
+ if(rawx.getc()==0x59)//assess data package frame header 0x59
+ {
+ uartx[1]=0x59;
+ for(i=2;i<9;i++)//save data in array
+ {
+ uartx[i]=rawx.getc();
+ }
+ check=uartx[0]+uartx[1]+uartx[2]+uartx[3]+uartx[4]+uartx[5]+uartx[6]+uartx[7];
+ if(uartx[8]==(check&0xff))//verify the received data as per protocol
+ {
+
+
+ strength=uartx[4]+uartx[5]*256;//calculate signal strength value
+ distx=uartx[2]+uartx[3]*256;//calculate distance value in cm
+ //velocityx = distx - distxo; ////velocity in meters
+ //distxo = distx;
+ }
+ }
+ }
+ }
+
+ }
+void dist_y() ///////////*******use this function to get distance it has object of rawserial as argument*******////////////
+ {
+ int strength;//signal strength of LiDAR
+ int check;//save check value
+ int i;
+ int uarty[9];//save data measured by LiDAR
+
+ if (rawy.readable())//check if serial port has data input
+ {
+ if(rawy.getc()==0x59)//assess data package frame header 0x59
+ {
+ uarty[0]=0x59;
+ if(rawy.getc()==0x59)//assess data package frame header 0x59
+ {
+ uarty[1]=0x59;
+ for(i=2;i<9;i++)//save data in array
+ {
+ uarty[i]=rawy.getc();
+ }
+ check=uarty[0]+uarty[1]+uarty[2]+uarty[3]+uarty[4]+uarty[5]+uarty[6]+uarty[7];
+ if(uarty[8]==(check&0xff))//verify the received data as per protocol
+ {
+
+ strength=uarty[4]+uarty[5]*256;//calculate signal strength value
+ disty=uarty[2]+uarty[3]*256;//calculate distance value
+ //velocityy = disty - distyo; ////velocity in meters
+ //distyo = disty;
+ }
+
+ }
+ }
+ }
+
+ }
+ void dist_z() ///////////*******use this function to get distance it has object of rawserial as argument*******////////////
+ {
+ int strength;//signal strength of LiDAR
+ int check;//save check value
+ int i;
+ int uartz[9];//save data measured by LiDAR
+
+ if (rawz.readable())//check if serial port has data input
+ {
+ if(rawz.getc()==0x59)//assess data package frame header 0x59
+ {
+ uartz[0]=0x59;
+ if(rawz.getc()==0x59)//assess data package frame header 0x59
+ {
+ uartz[1]=0x59;
+ for(i=2;i<9;i++)//save data in array
+ {
+ uartz[i]=rawz.getc();
+ }
+ check=uartz[0]+uartz[1]+uartz[2]+uartz[3]+uartz[4]+uartz[5]+uartz[6]+uartz[7];
+ if(uartz[8]==(check&0xff))//verify the received data as per protocol
+ {
+ strength=uartz[4]+uartz[5]*256;//calculate signal strength value
+ distz=uartz[2]+uartz[3]*256;//calculate distance value
+ //velocityz = distz - distzo; ////velocity in meters
+ //distzo = distz;
+ }
+ }
+ }
+ }
+
+ }
+ //////////////////////////////////**********velocity**************//////////////////////////////////////
+
+
+/////////////////////////////////***********************mpu****************************///////////////////////////////
+
+ void mpu_init()
+ {
+ i2c.frequency(100000);
+ char data_write[2];
+
+ data_write[0] = MPU6050_PWR_MGMT_1;
+ data_write[1] = 0x00;
+
+ int status = i2c.write(MPU6050_ADDR, data_write, 2, 0);
+
+ if (status != 0)
+ { // Error
+ while (1)
+ {
+ myled = !myled;
+ wait(0.2);
+ }
+ }
+ data_write[0] = USER_CTRL;
+ data_write[1] = I2C_MST_EN;
+ i2c.write(MPU6050_ADDR, data_write, 2, 0);
+
+ data_write[0] = 0x37;//INT_PIN_CFG;
+ data_write[1] = 0x02;
+ i2c.write(MPU6050_ADDR, data_write, 2, 0);
+
+ data_write[0] = MPU6050_SMPLRT_DIV;
+ data_write[1] = 0x07;
+ i2c.write(MPU6050_ADDR, data_write, 2, 0);
+
+ data_write[0] = MPU6050_ACCEL_CONFIG;
+ data_write[1] = 0x00;
+ i2c.write(MPU6050_ADDR, data_write, 2, 0);
+ data_write[0] = 0x1d;
+ data_write[1] = 0x06;
+ i2c.write(MPU6050_ADDR, data_write, 2, 0);
+ data_write[0] = MPU6050_CONFIG;
+ data_write[1] = 0x00;
+ i2c.write(MPU6050_ADDR, data_write, 2, 0);
+
+ data_write[0] = MPU6050_GYRO_CONFIG;
+ data_write[1] = 0x00;
+ i2c.write(MPU6050_ADDR, data_write, 2, 0);
+ wait(0.2);
+ }
+
+void mpu_read(){
+ char Rec_Data[14];
+ int16_t Accel_X_RAW = 0;
+ int16_t Accel_Y_RAW = 0;
+ int16_t Accel_Z_RAW = 0;
+ int16_t Temp_raw = 0;
+ int16_t Gyro_X_RAW = 0;
+ int16_t Gyro_Y_RAW = 0;
+ int16_t Gyro_Z_RAW = 0;
+ char data_write[1];
+
+ // Read 1 BYTES of data starting from ACCEL_XOUT_H register
+ data_write[0] = ACCEL_XOUT_H_REG;
+ i2c.write(MPU6050_ADDR, data_write, 1, 1); // no stop
+ i2c.read(MPU6050_ADDR, Rec_Data, 14);
+ Accel_X_RAW = (int16_t)(Rec_Data[0] << 8 | Rec_Data [1]);
+ Accel_Y_RAW = (int16_t)(Rec_Data[2] << 8 | Rec_Data [3]);
+ Accel_Z_RAW = (int16_t)(Rec_Data[4] << 8 | Rec_Data [5]);
+
+ Temp_raw = (int16_t)(Rec_Data[6] << 8 | Rec_Data [7]);
+
+ Gyro_X_RAW = (int16_t)(Rec_Data[8] << 8 | Rec_Data [9]);
+ Gyro_Y_RAW = (int16_t)(Rec_Data[10] << 8 | Rec_Data [11]);
+ Gyro_Z_RAW = (int16_t)(Rec_Data[12] << 8 | Rec_Data [13]);
+ /*** convert the RAW values into acceleration in 'g'
+ we have to divide according to the Full scale value set in FS_SEL
+ I have configured FS_SEL = 0. So I am dividing by 16384.0
+ for more details check ACCEL_CONFIG Register ****/
+
+ Ax = Accel_X_RAW/16384.0;
+ Ay = Accel_Y_RAW/16384.0;
+ Az = Accel_Z_RAW/16384.0;
+
+ Gx = Gyro_X_RAW/131.0;
+ Gy = Gyro_Y_RAW/131.0;
+ Gz = Gyro_Z_RAW/131.0;
+
+ }
+
+
+ void get_angle(){
+
+ mpu_read();
+ dt2=timer2.read_ms(); ////////****very imp self note check the order of dt1 and dt2*********///////////
+ dt=dt2-dt1;
+ dt=dt/1000;
+
+ roll_acc = (atan2((Ay),sqrt(pow((Ax),2) + pow((Az),2))))*(90*7/11);
+ pitch_acc = ((atan2((-Ax),sqrt(pow((Ay),2) + pow((Az),2))))*(90*7/11));
+
+ /* COMPLIMENTARY FILTER*/
+ pitch = Alfa_comp*(pitch + Gy*dt) + (1-Alfa_comp)*pitch_acc ;
+
+ roll = Alfa_comp*(roll + Gx*dt) + (1-Alfa_comp)*roll_acc ;
+ if(abs(Gz)>1)
+ yaw += Gz*dt;
+ dt1=timer2.read_ms();
+
+ }
+ void mpu_calibrate(void){
+ int i;
+ for(i=0;i<=3000;i++)
+ {
+ //dt2=timer2.read_ms();
+ if(i>0){
+ get_angle();}
+
+ // dt1=timer2.read_ms();
+ //dt=dt1-dt2;
+
+ avgx+=roll;
+ avgy+=pitch;
+ //avgz+=yaw;
+ }
+ avgx=avgx/3000;
+ avgy=avgy/3000;
+ // avgz=avgz/2000;
+ }
+ /*void Mag_init(){
+ char raw[3];
+ char data_write[2];
+
+///////////////////////////////////////// Power down magnetometer///////////////////////////////////////////////////////
+ data_write[0] = AK8963_CNTL;
+ data_write[1] = 0x00;
+
+ int status = i2c.write(AK8963_ADDRESS, data_write, 2, 0);
+ if (status != 0)
+ { // Error
+ while (1)
+ {
+ myled = !myled;
+ wait(0.2);
+ }
+ }
+
+ wait(0.01);
+ data_write[0] = 0x6B; //////////////////rest magnetometer
+ data_write[1] = 0x80;
+
+ i2c.write(MPU6050_ADDR, data_write, 2, 0);
+
+
+ wait(0.01);
+///////////////////////////////////////// Enter Fuse ROM access mode///////////////////////////////////////////////////
+ data_write[0] = AK8963_CNTL;
+ data_write[1] = 0x0F;
+ i2c.write(AK8963_ADDRESS, data_write, 2, 0);
+
+ wait(0.01);
+///////////////////////////////////////// Read the x-, y-, and z-axis calibration values//////////////////////////////
+ data_write[0] = AK8963_ASAX;
+ i2c.write(AK8963_ADDRESS, data_write, 1, 0);
+ i2c.read(AK8963_ADDRESS, raw, 3);
+// Return x-axis sensitivity adjustment values, etc.
+ H_senx = (float)(raw[0] - 128)/256.0f + 1.0f;
+ H_seny = (float)(raw[1] - 128)/256.0f + 1.0f;
+ H_senz = (float)(raw[2] - 128)/256.0f + 1.0f;
+
+ data_write[0] = AK8963_CNTL;
+ data_write[1] = 0x16;
+ i2c.write(AK8963_ADDRESS, data_write, 2, 0);
+ wait(0.01);
+ data_write[0] = 0x0B;
+ data_write[1] = 0x01;
+ i2c.write(AK8963_ADDRESS, data_write, 2, 0);
+ wait(0.01);
+ // Configure the magnetometer for continuous read and highest resolution
+ // set Mscale bit 4 to 1 (0) to enable 16 (14) bit resolution in CNTL register,
+ // and enable continuous mode data acquisition Mmode (bits [3:0]), 0010 for 8 Hz and 0110 for 100 Hz sample rates
+
+ }
+void Mag_read(){
+
+ char data_write[1];
+ char x[1];
+ char rawData[7];
+ /////////////read name///////////////////////////////////
+data_write[0] = WHO_AM_I_AK8963;
+ i2c.write(AK8963_ADDRESS, data_write, 1, 0);
+ i2c.read(AK8963_ADDRESS, name, 1);
+
+ //////////////////////////////////////
+ data_write[0] = AK8963_ST1;
+ i2c.write(AK8963_ADDRESS, data_write, 1, 0);
+ i2c.read(AK8963_ADDRESS, x, 1);
+
+ if(x[0] & 0x01) { // wait for magnetometer data ready bit to be set
+ data_write[0] = AK8963_XOUT_L;// Read the six raw data and ST2 registers sequentially into data array
+ i2c.write(AK8963_ADDRESS, data_write, 1, 1);
+ i2c.read(AK8963_ADDRESS, rawData, 7);
+
+ uint8_t c = rawData[6]; // End data read by reading ST2 register
+ if(!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data
+ Mag_X_RAW = (int16_t)((rawData[1] << 8) | rawData[0]); // Turn the MSB and LSB into a signed 16-bit value
+ Mag_Y_RAW = (int16_t)((rawData[3] << 8) | rawData[2]) ; // Data stored as little Endian
+ Mag_Z_RAW = (int16_t)((rawData[5] << 8) | rawData[4]) ;
+ }
+ }
+ Mx = Mag_X_RAW*H_senx;
+ My = Mag_Y_RAW*H_seny;
+ Mz = Mag_Z_RAW*H_senz;
+ }*/
\ No newline at end of file