Diff: main.cpp

Revision:

19:29d3c7caea66

Parent:

18:9b9ec0b35b45

Child:

20:babcf777607b

--- a/main.cpp	Tue Nov 19 00:42:38 2019 +0000
+++ b/main.cpp	Tue Nov 19 03:14:08 2019 +0000
@@ -42,6 +42,12 @@
 #include <iostream>
 #include <cmath>
 
+
+
+struct Vec3 {
+    float x, y, z;
+};
+
 /* Instantiate the expansion board */
 static XNucleoIKS01A2 *mems_expansion_board = XNucleoIKS01A2::instance(D14, D15, D4, D5);
 
@@ -53,27 +59,21 @@
 
 
 
-
-struct Vec3 {
-    float x, y, z;
-};
+DigitalOut M1_step(D3);
+DigitalOut M1_dir(D2);
 
+DigitalOut M2_step(D5);
+DigitalOut M2_dir(D4);
 
-static const Vec3 gyro_calibrate = {-0.42f, -1.61f, 1.05f};
+DigitalIn button1(D6);
+DigitalIn button2(D7);
 
-Vec3 get_gyro()
+void delay(float time)      //delay function  debounce buttons
 {
-    Vec3 vec3;
-
-    int32_t axes[3];
-    acc_gyro->get_g_axes(axes);
+    volatile int i;
+    for(i=0; i<1000000*time; i++);
+}
 
-    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;
-}
 
 Vec3 get_accel()
 {
@@ -88,18 +88,18 @@
     return vec3;
 }
 
-
-
 void print_vec3(const char* str, const Vec3& vec3)
 {
     std::cout << str << vec3.x << " " << vec3.y << " " << vec3.z << "\r\n";
 }
 
 
-/* Simple main function */
+
+
+
+
 int main()
 {
-
     /* Enable all sensors */
     magnetometer->enable();
     accelerometer->enable();
@@ -109,49 +109,114 @@
     //Timer t;
     //float dt = 0.02f;
 
-    std::cout << "\r\n--- Starting new run ---\r\n";
+    //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;
 
+    M1_dir = 1;
+    M2_dir = 0;
+
 
     for(;;) {
-        //t.start();
+        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;
 
 
-        //Vec3 gyro_reading = get_gyro();
+        int numOfSteps = degree / 0.45;
 
-        //orientation.x += (gyro_reading.x * dt * sens);
-        //orientation.y += (gyro_reading.y * dt * sens);
-        //orientation.z += (gyro_reading.z * dt * sens);
+        if(tilt.y > 0) {
+            M1_dir = 0;
+            M2_dir = 1;
+        } else {
+            M1_dir = 1;
+            M2_dir = 0;
+        }
 
-        //print_vec3("orientation: ", orientation);
-        
-        
-        
-        vec3 = get_accel();
-        
-        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;
+        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);
+        }
+    }
+
+}
+
 
 
 
 
-        print_vec3("acceleration: ", tilt);
+
+
+
+
+
+
+
+
+
+
+
+
 
-        std::cout << "\r\n" << std::flush;
+
+
+
+
+
+
+
+
+
+
+
+
 
-        t.stop();
-        if (dt - t.read() > 0) wait(dt - t.read());
-        dt = t.read();
-    }
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
 
 
 
@@ -161,30 +226,120 @@
 
 
 
-    //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";
+//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;
+//}
 
 
-    //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());
-    //std::cout << "\r\n" << std::flush;
+/* 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
+
+
 
 
-    //wait(1.5);
-    //}
-}
+//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);
+//}
+//}