Diff: main.cpp

Revision:

20:babcf777607b

Parent:

19:29d3c7caea66

Child:

21:b0e9b4d19d4d

diff -r 29d3c7caea66 -r babcf777607b main.cpp
--- a/main.cpp	Tue Nov 19 03:14:08 2019 +0000
+++ b/main.cpp	Fri Nov 22 23:54:51 2019 +0000
@@ -42,6 +42,12 @@
 #include <iostream>
 #include <cmath>
 
+const float MAX_SPEED = .002;
+const float MIN_SPEED = .003;
+const float MAX_DEGREES = 3.0;
+const float MIN_DEGREES = .5; 
+const float MAX_TILT = (MIN_SPEED - MAX_SPEED) / MAX_DEGREES;
+
 
 
 struct Vec3 {
@@ -65,14 +71,7 @@
 DigitalOut M2_step(D5);
 DigitalOut M2_dir(D4);
 
-DigitalIn button1(D6);
-DigitalIn button2(D7);
 
-void delay(float time)      //delay function  debounce buttons
-{
-    volatile int i;
-    for(i=0; i<1000000*time; i++);
-}
 
 
 Vec3 get_accel()
@@ -93,7 +92,10 @@
     std::cout << str << vec3.x << " " << vec3.y << " " << vec3.z << "\r\n";
 }
 
-
+float rad_to_deg(float radians)
+{
+    return radians * 57.2957795131f;
+}
 
 
 
@@ -106,240 +108,53 @@
     acc_gyro->enable_x();
     acc_gyro->enable_g();
 
-    //Timer t;
-    //float dt = 0.02f;
-
-    //std::cout << "\r\n--- Starting new run ---\r\n";
-
-    //Vec3 orientation = {};
-
-    //float sens = 0.488f;
-
-    Vec3 vec3, tilt;
-
-    // 180/pi -> converts radians to degrees.
-    float rad_to_deg = 57.2957795131f;
+    Vec3 vec3;
+    int numOfSteps;
 
     M1_dir = 1;
     M2_dir = 0;
 
+    float given;
+
 
     for(;;) {
         vec3 = get_accel();
-
-        // We only need x or y; I left the others here for documentation.
-        //tilt.x = atan(vec3.x / sqrtf(vec3.y*vec3.y + vec3.z*vec3.z)) * rad_to_deg;
-        tilt.y = atan(vec3.y / sqrtf(vec3.x*vec3.x + vec3.z*vec3.z)) * rad_to_deg;
-        //tilt.z = atan(sqrtf(vec3.y*vec3.y + vec3.x*vec3.x) / vec3.z) * rad_to_deg;
-
-//control stepper motor 1
-
-        float degree = std::abs(tilt.y);
-
-        //print_vec3("acceleration: ", tilt);
-        //std::cout << "\r\n" << std::flush;
-
-
-        int numOfSteps = degree / 0.45;
-
-        if(tilt.y > 0) {
-            M1_dir = 0;
-            M2_dir = 1;
-        } else {
-            M1_dir = 1;
-            M2_dir = 0;
-        }
+        given = rad_to_deg(atan(vec3.y / sqrtf(vec3.x*vec3.x + vec3.z*vec3.z)));
 
-        for(int i = 0; i < numOfSteps; i++) {
-            M2_step = 1;
-            M1_step = 1;
-            delay(0.005);
-            M1_step = 0;
-            M2_step = 0;
-            delay(0.005);
-        }
-    }
-
-}
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+        numOfSteps = given / 0.055f;
 
 
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+        if (std::abs(given) > MIN_DEGREES) {
+//Change direction.
+            if(given > 0) {
+                M1_dir = 0;
+                M2_dir = 1;
+            } else {
+                M1_dir = 1;
+                M2_dir = 0;
+            }
 
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+            float stepTime;
+            given = std::abs(given);
+            
+            if (given > MAX_DEGREES){
+                stepTime = MAX_SPEED;
+            }
+            else{
+                stepTime = (MAX_TILT * given) + MIN_SPEED;
+            }
+            //Control stepper motor.
+            for(int i = 0; i < numOfSteps; ++i) {
+                M2_step = 1;
+                M1_step = 1;
+                wait(0.0014);
+                M1_step = 0;
+                M2_step = 0;
+                wait(0.0014);
+            }
+        }
 
 
 
-
-
-
-
-
-
-
-
-
-//static const Vec3 gyro_calibrate = {-0.42f, -1.61f, 1.05f};
-
-//Vec3 get_gyro()
-//{
-//    Vec3 vec3;
-
-//    int32_t axes[3];
-//    acc_gyro->get_g_axes(axes);
-
-//    vec3.x = axes[0]/1000.0f - gyro_calibrate.x;
-//    vec3.y = axes[1]/1000.0f - gyro_calibrate.y;
-//    vec3.z = axes[2]/1000.0f - gyro_calibrate.z;
-
-//   return vec3;
-//}
-
-
-
-
-
-
-
-
-/* Simple main function */
-//int main()
-//{
-
-
-
-
-//for(;;) {
-//t.start();
-
-
-//Vec3 gyro_reading = get_gyro();
-
-//orientation.x += (gyro_reading.x * dt * sens);
-//orientation.y += (gyro_reading.y * dt * sens);
-//orientation.z += (gyro_reading.z * dt * sens);
-
-//print_vec3("orientation: ", orientation);
-
-
-
-
-
-
-
-
-
-
-
-//tilt.x
-
-
-
-
-//std::cout << "\r\n" << std::flush;
-
-//t.stop();
-//if (dt - t.read() > 0) wait(dt - t.read());
-//dt = t.read();
-//}
-
-
-
-
-
-
-
-
-
-//for(;;) {
-//-0.42 -1.61 1.05
-//magnetometer->get_m_axes(axes);
-//std::cout << "LSM303AGR [mag/gauss]: " << axes[0]/1000.0f << " " << axes[1]/1000.0f << " " << axes[2]/1000.0f << "\r\n";
-
-//accelerometer->get_x_axes(axes);
-//std::cout << "LSM303AGR [acc/g]: " << axes[0]/1000.0f << " " << axes[1]/1000.0f << " " << axes[2]/1000.0f << "\r\n";
-
-
-//acc_gyro->get_x_axes(axes);
-//std::cout << "LSM6DSL [acc/g]: " << axes[0]/1000.0f << " " << axes[1]/1000.0f << " " << axes[2]/1000.0f << "\r\n";
-
-
-//acc_gyro->get_g_axes(axes);
-//std::cout << "LSM6DSL [gyro/dps]: " << axes[0]/1000.0f + 0.42f << " " << axes[1]/1000.0f + 1.61f << " " << axes[2]/1000.0f-1.05f << "\r\n";
-
-
-
-
-
-//print_vec3("gyro/dps: ", get_gyro());
-//
-
-
-//wait(1.5);
-//}
-//}
+    }
+}
\ No newline at end of file