Diff: main.cpp

Revision:

0:66a265dc3146

Child:

2:a79a9c9f83a4

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