Diff: main.cpp

Revision:

0:3c1eead7416c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Dec 13 02:22:49 2019 +0000
@@ -0,0 +1,158 @@
+#include "ultrasonic.h"
+#include "motordriver.h"
+
+//ADXL
+SPI acc(p11,p12,p13); // set up SPI interface on pins 11,12,13
+DigitalOut cs(p14); // use pin 14 as chip select
+
+//FRONT MOTORS
+Motor FL(p22, p6, p5, 1); // pwm, fwd, rev, can brake 
+Motor FR(p21, p7, p8, 1); 
+
+//BACK MOTORS
+Motor BL(p23, p17, p18, 1); 
+Motor BR(p24, p15, p16, 1);
+
+void drivefront(void);
+void driveback(void);
+void driveleft(void);
+void driveright(void);
+void dynamicdrivefront(void);
+
+Serial pc(USBTX, USBRX);
+
+char buffer[6]; //raw data array type char
+int16_t data[3]; // 16-bit twos-complement integer data
+float x, y, z; // floating point data, to be displayed on-screen
+
+int sl,sr; //left sensor and right sensor
+void dist(int distance)
+{
+    
+    //put code here to execute when the distance has changed
+    
+  sl = distance;
+  pc.printf("sl %d mm\r\n", sl);
+}
+void gist(int distance)
+{
+    
+    //put code here to execute when the distance has changed
+    
+  sr = distance;
+  pc.printf("sr %d mm\r\n", sr);
+}
+ 
+ultrasonic mu(p27, p28, .1, 1, &gist);    //Set the trigger pin to p27 and the echo pin to p28
+ultrasonic lu(p10, p9, .1, 1, &dist);                                        //have updates every .1 seconds and a timeout after 1
+                                        //second, and call dist when the distance changes
+ 
+int main()
+{ 
+//ADXL{
+cs=1; //initially ADXL345 is not activated
+acc.format(8,3); // 8 bit data, Mode 3
+acc.frequency(2000000); // 2MHz clock rate
+cs=0; //select the device
+acc.write(0x31); // data format register
+acc.write(0x0B); // format +/-16g, 0.004g/LSB
+cs=1; //end of transmission
+cs=0; //start a new transmission
+acc.write(0x2D); // power ctrl register
+acc.write(0x08); // measure mode
+cs=1; //end of transmission
+//}ADXL
+    lu.startUpdates();//start measuring the distance    
+    mu.startUpdates();//start measuring the distance
+    while(1)
+    { 
+        cs=0; //start a transmission
+        acc.write(0x80|0x40|0x32); // RW bit high, MB bit high, plus address
+        for (int i = 0;i<=5;i++) {
+        buffer[i]=acc.write(0x00); // read back 6 data bytes
+        }
+        cs=1; //end of transmission
+        data[0] = buffer[1]<<8 | buffer[0]; // combine MSB and LSB
+        data[1] = buffer[3]<<8 | buffer[2];
+        data[2] = buffer[5]<<8 | buffer[4];
+        x=0.004*data[0]; y=0.004*data[1]; z=0.004*data[2]; // convert to float,
+        //actual g value
+
+      // pc.printf("x = %+1.2fg\t y = %+1.2fg\t z = %+1.2fg\n\r", x, y,z); //print 
+    
+     lu.checkDistance(); 
+        mu.checkDistance(); 
+        
+            if (sl>200 && sr>200){
+                            if(y>=0.26){//o.26=sin(15) to signify elevation above that point
+                            dynamicdrivefront();
+                            }
+                            else{
+                                drivefront();
+                            }
+                            }
+                           else if(sl >150 && sr<=150){
+                                driveleft();
+                                }
+                               else if(sr >150 && sl<=150){
+                                     driveright();
+                                     }
+                                   else{
+                                        driveback();
+                                        wait(0.6);
+                                        if(sl<sr){
+                                           driveright();}
+                                           else{
+                                                driveleft();
+                                                    }           
+                                                }
+                                        }
+        
+       
+                            
+    }
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+void drivefront(void)
+{ 
+FL.speed(0.4);
+FR.speed(0.4);
+BL.speed(0.4);
+BR.speed(0.4);
+//wait(0.1);
+}
+
+void driveback(void)
+{
+FL.speed(-0.4);
+FR.speed(-0.4);
+BL.speed(-0.4);
+BR.speed(-0.4);
+//wait(0.1);   
+    }
+void driveleft(void)
+{
+FL.speed(-0.5);
+FR.speed(0.4);
+BL.speed(-0.5);
+BR.speed(0.3);
+//wait(0.1);
+    }
+void driveright(void)
+{
+FL.speed(0.4);
+FR.speed(-0.5);
+BL.speed(0.3);
+BR.speed(-0.5);
+//wait(0.1);
+    }
+void dynamicdrivefront(void)
+{ 
+FL.speed(0.5+((y-0.1)/2));
+FR.speed(0.5+((y-0.1)/2));
+BL.speed(0.5+((y-0.1)/2));
+BR.speed(0.5+((y-0.1)/2));
+//wait(0.1);
+}    
\ No newline at end of file