Diff: main.cpp

Revision:

1:a58d5b79517e

Parent:

0:c1b0d03af2ac

Child:

2:418fc2dabdb5

--- a/main.cpp	Fri Nov 15 19:33:02 2019 +0000
+++ b/main.cpp	Fri Nov 15 23:59:16 2019 +0000
@@ -1,43 +1,33 @@
 #include "mbed.h"
+#include "math.h"
+#include "TextLCD.h"
+
 #define LENOFBOT 2.5
 #define LENOFSENSOR 0.394
-//object declarations
+//obj for distance sensor 
 Serial pc(USBTX, USBRX);
-Serial Ardu(p9,p10);
 PwmOut trigger(p24);
 PwmOut trigger_b(p23);
 InterruptIn echoStart(p13,PullDown);
 InterruptIn echoStop(p14,PullDown);
 Timer timer;
 Timer timer_b;
-//function prototypes
-void inline MPU6050Values(Serial *Ardu,int16_t *accy,int16_t *accz,int16_t *gyr, float *samplerate, int *k, float *offset,float *pitch, float *angleX); 
-float inline filteredAngle(float *pitch, float *gyrX);
-void start(void);
-void stop (void);
-void start_back(void);
-void stop_back (void);
-Timer timer;
-Timer timer_b;
+//functions for distance sensor
 volatile float front_reading;
 volatile float back_sensor;
-
-int main()
-{
-    //begining of distance sensor variables
-    volatile float front_reading;
-    volatile float back_sensor;
-    echoStart.rise(&start);
-    echoStart.fall(&stop);
-    echoStop.rise(&start_back);
-    echoStop.fall(&stop_back);
-    trigger.period(0.06);
-    int pi[]= {3,1,4,2};
-    int currentdigit =0;
-    int front_distance;
-    int back_distance;
-    //end of distance sensor variabled
-    //beginning of IMU variables
+void stop_back (void);
+void start_back(void);    
+void stop (void);
+void start(void);
+//functions for MPU
+Serial Ardu(p9,p10);
+void inline MPU6050Values(Serial *Ardu,int16_t *accy,int16_t *accz,int16_t *gyr, float *samplerate, int *k, float *offset,float *pitch, float *angleX); 
+float inline filteredAngle(float *pitch, float *gyrX);
+//LCD
+I2C i2c_lcd(p27,p28);
+TextLCD_I2C lcd(&i2c_lcd, 0x4E, TextLCD::LCD16x2);
+int main() {
+    //MPU variables
     uint8_t charArr[6];
     int16_t accy;
     int16_t accz;
@@ -50,57 +40,37 @@
     float gyr_with_ofset =0;
     int k =1; // this is for getting the initial angle the first time around so we can start at zero
     int j=0;
-    //End of IMU variables
-    
-    while (true) {
+    //distance sensor stuff
+    echoStart.rise(&start);
+    echoStart.fall(&stop);
+    echoStop.rise(&start_back);
+    echoStop.fall(&stop_back);
+    trigger.period(0.06);
+    int pi[]= {3,1,4,2};
+    int currentdigit =0;
+    int front_distance;
+    int back_distance;
+    while(1) {
         trigger.pulsewidth_us(10);
         front_distance = (3*pi[currentdigit])+9;
         back_distance= 27-front_distance;
-        
         MPU6050Values(&Ardu,&accy,&accz,&gyr, &samplerate, &k, &offset,&pitch, &angleX);
         finalAx = filteredAngle(&pitch, &angleX);
         
+        pc.printf("calc angle: %f.2\n\r",finalAx);
         if((front_reading/2)+LENOFBOT+LENOFSENSOR>front_distance){
-        pc.printf("moving back distance read: %.3f digit: %d\n\r",(front_reading/2)+LENOFBOT+LENOFSENSOR,pi[currentdigit]);
+        pc.printf("moving back distance read: %.3f digit: %d\n\r",(front_reading/2)+LENOFBOT+LENOFSENSOR,pi[1]);
+        //lcd.printf("%.3f\n\r" , (front_reading/2)+LENOFBOT+LENOFSENSOR);
         if(((back_sensor/2)+LENOFBOT+LENOFSENSOR <= back_distance-0.1||(back_sensor/2)+LENOFBOT+LENOFSENSOR <= back_distance+0.1))
-            currentdigit++;
+            int r =1;
+            //currentdigit++;
         }else if((front_reading/2)+LENOFBOT+LENOFSENSOR<=front_distance){
-            pc.printf("moving forward distance read: %.3f should be %d\n\r",(front_reading/2)+LENOFBOT+LENOFSENSOR, pi[currentdigit]);
+            pc.printf("moving forward distance read: %.3f should be %d\n\r",(front_reading/2)+LENOFBOT+LENOFSENSOR, pi[0]);
         if((back_sensor/2)+LENOFBOT+LENOFSENSOR <= back_distance-0.1||(back_sensor/2)+LENOFBOT+LENOFSENSOR <= back_distance+0.1)
-            currentdigit++;
-        if(j=1000){
-        pc.printf("calc angle: %f.2\n\r",finalAx);
-        j=0;
-        }else
-        j++;    
-        
- }
-}
-
-
-
-
-
-//function definitions 
-inline void MPU6050Values(Serial *Ardu,int16_t *accy,int16_t *accz,int16_t *gyr, float *samplerate, int *k, float *offset,float *pitch, float *angleX){
-    uint8_t charArr[6];
-    for(int i =0; i<6;i++)
-        charArr[i]=Ardu->getc();
-    
-    (*accy)= charArr[0]<<8|charArr[1];
-    (*accz)= charArr[2]<<8|charArr[3];
-    (*gyr)= charArr[4]<<8|charArr[5];
-    (*pitch) = atan((*accy)/(*accz));
-    if(*k){
-        (*offset) = (*gyr);//gets the offset or starting point the first time around
-        *k=0;
+            int i=1;
+            //currentdigit++;
         }
-        
-    (*angleX) += ((*gyr) - (*offset))/(*samplerate);
     }
-
-inline float filteredAngle(float *pitch, float *angleX){
-    return (*pitch)*(0.02)+(*angleX)*(0.98);
 }
 
 void start(void){
@@ -119,4 +89,27 @@
     timer_b.stop();
     back_sensor = (float)timer_b.read()*13480;
     timer_b.reset();
+    }
+    
+    
+inline void MPU6050Values(Serial *Ardu,int16_t *accy,int16_t *accz,int16_t *gyr, float *samplerate, int *k, float *offset,float *pitch, float *angleX){
+    uint8_t charArr[6];
+    for(int i =0; i<6;i++)
+        charArr[i]=Ardu->getc();
+    
+    (*accy)= charArr[0]<<8|charArr[1];
+    (*accz)= charArr[2]<<8|charArr[3];
+    (*gyr)= charArr[4]<<8|charArr[5];
+    float thing=(*accy)/(*accz);
+    (*pitch) = atan(thing);
+    if(*k){
+        (*offset) = (*gyr);//gets the offset or starting point the first time around
+        *k=0;
+        }
+        
+    (*angleX) += ((*gyr) - (*offset))/(*samplerate);
+    }
+
+inline float filteredAngle(float *pitch, float *angleX){
+    return (*pitch)*(0.02)+(*angleX)*(0.98);
     }
\ No newline at end of file