Diff: main.cpp

Revision:

0:9e0de96e4dfd

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Dec 13 21:30:04 2018 +0000
@@ -0,0 +1,111 @@
+//Nicholas Restivo
+#include "mbed.h"
+#include "PwmIn.h"
+#include "Servo.h"
+#include "LSM9DS1.h"
+
+#define PI 3.14159
+
+Servo Rout(p22);
+Servo Lout(p21);
+
+Serial pc(USBTX, USBRX);
+
+PwmIn elev(p16);
+PwmIn right(p17);
+PwmIn left(p18);
+float Rin = 0;
+float Lin = 0;
+float Ein = 0;
+bool RCon = true;
+
+float pitch;
+
+LSM9DS1 IMU(p28, p27, 0xD6, 0x3C);
+
+
+float getAttitude(float ax, float ay, float az)
+{
+    float roll = atan2(ay, az);
+    float pitch_res = atan2(-ax, sqrt(ay * ay + az * az));
+
+    // Convert everything from radians to degrees:
+    pitch_res *= 180.0 / PI;
+    roll  *= 180.0 / PI;
+    if (pitch_res < -3.0 || pitch_res > 3.0) pc.printf("Pitch: %f,    Roll: %f degress\n\r",pitch_res,roll);
+    return pitch_res;
+}
+
+int main() {
+    
+    Rout.calibrate(0.0009, 90.0);
+    Lout.calibrate(0.0009, 90.0);
+    
+    //Accelerometer
+    IMU.begin();
+    if (!IMU.begin()) {
+        pc.printf("Failed to communicate with LSM9DS1.\n");
+    }
+    IMU.calibrate(1);
+    while(!IMU.accelAvailable());
+    IMU.readAccel();
+    pitch = getAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az));
+    
+    while(1) {
+        
+        if (RCon) {
+        
+            printf("Duty Cycle: %f \n", Ein);
+            
+            // Input Values & Round
+            Lin = float(int(left.dutycycle() * 1000 + 0.5)) / 100; //round to 100th & shift - range is .75 - 1
+            Rin = float(int(right.dutycycle() * 1000 + 0.5)) / 100; //round to 100th & shift - range is .75 - .96
+            Ein = float(int(elev.dutycycle() * 1000 + 0.5)) / 100; //round to 100th & shift - range is .74 -  .53
+            
+            //Scale to controller
+            Lin = 1.0 - (Lin - 0.75) * 4.0; // 1 when stick is centered
+            Rin = (Rin - 0.75) * 4.7619; // 0 when stick is centered
+            Rin = float(int(Rin * 100 + 0.5)) / 100;
+    //        if (Ein > 0.65) {
+    //            Ein = 0.0;
+    //        }
+            if (Ein > 0 && Ein <= 0.65) {
+                Ein = (1.0 - (Ein - .53) * 8.333); 
+                Ein = float(int(Ein * 100 + 0.5)) / 100; // 0 when centered (and a little below)
+                Lin = 1.0 - Ein;
+                Rin = Ein;
+            }
+            
+            Lout.write(Lin); //float percent .75 - 1... -> 0 - .25 -> 0 - 1.0
+            Rout.write(Rin); //float percent .75 - .96... -> 0 - .21 -> 0 - 1.0
+            
+        }
+        
+        else {
+            //To-Do!
+            //integrate accelerometer readings to allow autonomous control
+            //while(!IMU.accelAvailable());
+            IMU.readAccel();
+            pitch = getAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az));
+            if (pitch > 15.0) {
+                Rout.write(1.0);
+            }
+            else if (pitch < -15.0) {
+                Lout.write(0.0);
+            }
+            else {
+                Rout.write(0.0);
+                Lout.write(1.0);
+            }
+            
+        }
+        
+        Lin = float(int(left.dutycycle() * 1000 + 0.5)) / 100; //round to 100th & shift - range is .75 - 1
+        if (Lin == 0.0) {
+            RCon = false;
+        }
+        else {
+            RCon = true;
+        }
+    }
+}