Diff: main.cpp

Revision:

0:caa8ee3bd882

Child:

2:df0c6af898ac

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Nov 12 00:14:05 2017 +0000
@@ -0,0 +1,328 @@
+#include "main.h"
+
+// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
+// UI implementation
+// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
+
+void ui::StatusDisplay()
+{
+    switch (robot::state) {
+        case robot::OFF: // continuous red;
+            rgb_led[0] = LED_ON;
+            rgb_led[1] = !LED_ON;
+            rgb_led[2] = !LED_ON;
+            break;
+        case robot::CALIBRATION: // alternating red-green; busy
+            rgb_led[0] = !rgb_led[0];
+            rgb_led[1] = !rgb_led[0];
+            rgb_led[2] = !LED_ON;
+            break;
+        case robot::HOMING: // alternating green-blue; busy
+            rgb_led[0] = !LED_ON;
+            rgb_led[1] = !rgb_led[1];
+            rgb_led[2] = !rgb_led[1];
+            break;
+        case robot::READY: // continuous green
+            rgb_led[0] = !LED_ON;
+            rgb_led[1] = LED_ON;
+            rgb_led[2] = !LED_ON;
+            break;
+        case robot::DEMO: // alternating red-blue; busy
+            rgb_led[0] = !rgb_led[0];
+            rgb_led[1] = !LED_ON;
+            rgb_led[2] = !rgb_led[0];
+            break;
+        case robot::MANUAL: // alternating green-purple
+            rgb_led[0] = !rgb_led[0];
+            rgb_led[1] = !rgb_led[0];
+            rgb_led[2] = rgb_led[0];
+            break;
+    }
+}
+
+
+
+void ui::SwitchState(State new_state)
+{
+    ui::serial.printf("* Switching Input State to: %s\r\n",StateNames[new_state]);
+    ui::state = new_state;
+}
+
+void robot::SwitchState(State new_state)
+{
+    ui::serial.printf("* Switching Robot State to: %s\r\n",StateNames[new_state]);
+    state = new_state;
+}
+
+void ui::InterruptSwitch2()
+{
+    if (robot::has_power()) {
+        robot::Stop();
+    } else {
+        robot::Start();
+    }
+}
+
+void ui::InterruptSwitch3()
+{
+    switch (state) {
+        case STATE_SWITCHING:
+            robot::GoToNextState();
+            break;
+    }
+}
+
+
+
+
+// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
+// Robot implementation
+// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
+
+
+void robot::GoToNextState()
+{
+    switch (state) {
+        case OFF:
+            SwitchState(CALIBRATION);
+            ui::SwitchState(ui::IGNORE);
+            Start();
+            break;
+        case CALIBRATION:
+            SwitchState(HOMING);
+            Start();
+            break;
+        case HOMING:
+            SwitchState(DEMO);
+            //ui::SwitchState(ui::STATE_SWITCHING);
+            break;
+        case READY:
+            SwitchState(DEMO);
+            break;
+        case DEMO:
+            SwitchState(MANUAL);
+            break;
+        default:
+            SwitchState(OFF);
+            robot::is_calibrated = false;
+            Stop();
+    }
+}
+
+bool robot::has_power()
+{
+    return m1.has_power() or m2.has_power();
+}
+
+double robot::get_angle(int i)
+{
+    switch(i) {
+        case 1:
+            return m1.get_angle();
+        case 2:
+            return m2.get_angle();
+        default:
+            return NULL;
+    }
+}
+
+double robot::ForwardKinematicsX(double theta_1, double theta_2)
+{
+    return kL1*cos(deg2rad(theta_1)) + kL2*cos(deg2rad(theta_2));
+}
+
+double robot::ForwardKinematicsY(double theta_1, double theta_2)
+{
+    return kL1*sin(deg2rad(theta_1)) + kL2*sin(deg2rad(theta_2));
+}
+
+double robot::get_x()
+{
+    return ForwardKinematicsX(get_angle(1),get_angle(2));
+}
+
+double robot::get_y()
+{
+    return ForwardKinematicsY(get_angle(1),get_angle(2));
+}
+
+double robot::InverseKinematicsTheta(double x, double y, int i)
+{
+    double r = sqrt(pow(x,2) + pow(y,2));
+    double dir_angle = rad2deg( atan2(y,x) );
+    
+    double int_angle_1 = rad2deg( acos((pow(kL1,2) + pow(r,2) - pow(kL2,2))/(2*kL1*r)) );
+    double int_angle_2 = rad2deg( acos((pow(kL2,2) + pow(r,2) - pow(kL1,2))/(2*kL2*r)) );
+    
+    double theta_1 = dir_angle + int_angle_1;
+    double theta_2 = dir_angle - int_angle_2;
+    
+    switch (i) {
+        case 1: return theta_1;
+        case 2: return theta_2;
+        default: return NULL;
+    }
+}
+
+void robot::Start()
+{
+    ui::serial.printf("  Setting current position as reference.\r\n");
+    ref::SetPositionAsReference();
+
+    ui::serial.printf("  Resetting controllers.\r\n");
+    c1.Reset();
+    c2.Reset();
+
+    ui::serial.printf("  Starting motors.\r\n");
+    m1.Start();
+    m2.Start();
+}
+
+void robot::Stop()
+{
+    ui::serial.printf("  Stopping motors.\r\n");
+    m1.Stop();
+    m2.Stop();
+}
+
+void robot::ControlLoop()
+{
+    m1.set_pwm(c1.Update( ref::theta_1 - get_angle(1) ));
+    m2.set_pwm(c2.Update( ref::theta_2 - get_angle(2) ));
+}
+
+void robot::SetCalibrationAngles()
+{
+    m1.set_angle(kCalibAngleMotor1);
+    m2.set_angle(kCalibAngleMotor2);
+    is_calibrated = true;
+}
+
+// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
+// Reference implementation
+// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
+
+double ref::get_ref_x()
+{
+    return robot::ForwardKinematicsX(theta_1,theta_2);
+}
+
+double ref::get_ref_y()
+{
+    return robot::ForwardKinematicsY(theta_1,theta_2);
+}
+
+void ref::UpdateReference()
+{
+    switch(robot::state) {
+        case robot::CALIBRATION:
+            CalibrationReference();
+            break;
+        case robot::HOMING:
+            HomingReference();
+            break;
+        case robot::DEMO:
+            DemoReference();
+            break;
+    }
+}
+
+void ref::CalibrationReference()
+{
+    // Calibration process
+    bool is_ready_1 = (theta_1 - robot::get_angle(1) > kCalibrationError);
+    bool is_ready_2 = (theta_2 - robot::get_angle(2) > kCalibrationError);
+
+    if (is_ready_1) {
+        robot::m1.Stop();
+    } else {
+        theta_1 += kCalibrationOmegaStep;
+    }
+
+    if (is_ready_2) {
+        robot::m2.Stop();
+    } else {
+        theta_2 += kCalibrationOmegaStep;
+    }
+
+    if (is_ready_1 & is_ready_2) {
+        robot::Stop();
+        ui::serial.printf("+ Both motors at end-stop.\r\n");
+        robot::SetCalibrationAngles();
+        robot::GoToNextState();
+    }
+}
+
+void ref::HomingReference()
+{
+    double dist_1 = kOriginTheta1 - theta_1;
+
+    if (abs(dist_1)<=kCalibrationOmegaStep) {
+        // motor 1 ready
+        theta_1 = kOriginTheta1;
+        
+        double dist_2 = kOriginTheta2 - theta_2;
+
+        if (abs(dist_2)<=kCalibrationOmegaStep) {
+            // motor 2 also ready
+            theta_2 = kOriginTheta2;
+            
+            ui::serial.printf("  Robot at home position.\r\n");
+            robot::GoToNextState();
+        } else {
+            // motor 2 still to move
+            theta_2 += dist_2 / abs(dist_2) * kCalibrationOmegaStep;
+        }
+
+
+    } else {
+        // motor 1 still to move
+        theta_1 += dist_1 / abs(dist_1) * kCalibrationOmegaStep;
+    }
+
+
+}
+
+void ref::DemoReference()
+{
+    double x = robot::ForwardKinematicsX(theta_1,theta_2);
+    double y = robot::ForwardKinematicsY(theta_1,theta_2);
+    
+    double x_goal = kDemoCoords[0][i_demo_coord];
+    double y_goal = kDemoCoords[1][i_demo_coord];
+    
+    double delta_x = x_goal - x;
+    double delta_y = y_goal - y;
+    
+    double dist = sqrt(pow(delta_x,2) + pow(delta_y,2));
+    
+    double x_new = x;
+    double y_new = y;
+    
+    if (dist<kMaxStep) {
+        x_new = x_goal;
+        y_new = y_goal;
+        
+        ui::serial.printf("  Reached Demo Coord %d\r\n",i_demo_coord);
+        i_demo_coord = ++i_demo_coord % kNumDemoCoords;
+        
+        if (i_demo_coord == 0) {
+            robot::is_calibrated = false;
+            robot::SwitchState(robot::CALIBRATION);
+        }
+    } else {
+        x_new += delta_x / dist * kMaxStep;
+        y_new += delta_y / dist * kMaxStep;
+    }
+    
+    theta_1 = robot::InverseKinematicsTheta(x_new, y_new, 1);
+    theta_2 = robot::InverseKinematicsTheta(x_new, y_new, 2);
+    
+    
+}
+
+void ref::SetPositionAsReference()
+{
+    ref::theta_1 = robot::get_angle(1);
+    ref::theta_2 = robot::get_angle(2);
+}
\ No newline at end of file