Diff: main.cpp

Revision:

21:b0e9b4d19d4d

Parent:

20:babcf777607b

Child:

22:5e7356dc1d71

--- a/main.cpp	Fri Nov 22 23:54:51 2019 +0000
+++ b/main.cpp	Sat Nov 23 00:55:46 2019 +0000
@@ -42,11 +42,18 @@
 #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;
+const float MAX_SPEED = 0.0014f;
+const float MIN_SPEED = 0.1f;
+
+const float MAX_DEGREES = 5.0f;
+const float MIN_DEGREES = 0.0f;
+
+
+
+float degrees_to_period(float degrees)
+{
+    return (((MAX_SPEED - MIN_SPEED)/(MAX_DEGREES)) * std::abs(degrees) + MIN_SPEED);
+}
 
 
 
@@ -65,13 +72,16 @@
 
 
 
-DigitalOut M1_step(D3);
+PwmOut M1_step(D3);
 DigitalOut M1_dir(D2);
 
-DigitalOut M2_step(D5);
+PwmOut M2_step(D5);
 DigitalOut M2_dir(D4);
 
 
+Serial out(USBTX, USBRX);
+
+
 
 
 Vec3 get_accel()
@@ -109,52 +119,36 @@
     acc_gyro->enable_g();
 
     Vec3 vec3;
-    int numOfSteps;
 
     M1_dir = 1;
     M2_dir = 0;
+    
+    M1_step = 0.5f;
+    M2_step = 0.5f;
 
-    float given;
+    float angle;
+    float period;
 
 
     for(;;) {
         vec3 = get_accel();
-        given = rad_to_deg(atan(vec3.y / sqrtf(vec3.x*vec3.x + vec3.z*vec3.z)));
-
-        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);
-            }
-        }
+        angle = rad_to_deg(atan(vec3.y / sqrtf(vec3.x*vec3.x + vec3.z*vec3.z)));
+        period = (degrees_to_period(angle));
 
 
 
+//Change direction.
+        if(angle > 0) {
+            M1_dir = 0;
+            M2_dir = 1;
+        } else {
+            M1_dir = 1;
+            M2_dir = 0;
+        }
+
+        //Control stepper motor.
+        out.printf("%f %f\r\n", period, angle);
+        M1_step.period(period);
+        M2_step.period(period);
     }
 }
\ No newline at end of file