Diff: main.cpp

Revision:

7:b9a209f889f5

Parent:

6:cddc73091ab5

Child:

9:4bf2034d37a3

--- a/main.cpp	Mon Nov 13 09:34:39 2017 +0000
+++ b/main.cpp	Mon Nov 13 10:39:55 2017 +0000
@@ -1,428 +1,70 @@
-#include "main.h"
-
-// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
-
 /**
- * UI implementation
- */
-namespace ui
-{
-
-/**
- * Display robot state using (flashing) LEDs
+ * Code to bring BioRobotics2017 group 20's robot to life.
+ *
+ * M.E. Grootens, UTwente, v0.2 11-12-2017
+ *
+ * DISCLAIMER: 
+ * Just a quick-and-dirty working-example. I do NOT claim this to be an
+ * example of good coding practice... as it is not.
+ * Motors and Controllers are implemented as C++ classes. Reference, UI and 
+ * Robot are kept in different namespaces, so as to avoid too many global 
+ * variables.
+ *
+ * THE PROGRAM:
+ * The robot starts to calibrate when SW3 is pressed and will then cycle
+ * through the following states:
+ * [OFF->CALIBRATION->HOMING->DEMO->OFF->CALIBRATION->etc.]
+ * The robot will move continuously; MANUAL mode is not implemented.
+ *
+ * Implementation can be found in main.cpp
  */
-void 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;
-    }
-}
+#include "mbed.h"
 
-
-/**
- * Switching UI state to a new state
- */
-void SwitchState(State new_state)
-{
-    serial.printf("* Switching INPUT State to: %s\r\n",StateNames[new_state]);
-    state = new_state;
-}
-
-
-void InterruptSwitch2()
-{
-    if (robot::has_power()) {
-        robot::Stop();
-    } else {
-        robot::Start();
-    }
-}
-
-void InterruptSwitch3()
-{
-    switch (state) {
-        case STATE_SWITCHING:
-            robot::GoToNextState();
-            break;
-    }
-}
-
-} // end namespace ui
-
+#include "tools.h"
+#include "ui.h"
+#include "robot.h"
+#include "ref.h"
 
 
 
-// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
-
 /**
- * Robot implementation
- */
-
-namespace robot
-{
-
-/**
- * Switching ROBOT state to a new state
- */
-void SwitchState(State new_state)
-{
-    ui::serial.printf("* Switching ROBOT State to: %s\r\n",StateNames[new_state]);
-    state = new_state;
-}
-
-/**
- * State flow // MANUAL state not implemented
- */
-void 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;
-        default:
-            SwitchState(OFF);
-            is_calibrated = false;
-            Stop();
-    }
-}
-
-/**
- * Check whether any of the motors is running
- */
-bool has_power()
-{
-    return m1.has_power() or m2.has_power();
-}
-
-/**
- * Get motor angle in [deg] for motor i=1,2
- */
-double get_angle(int i)
-{
-    switch(i) {
-        case 1:
-            return m1.get_angle();
-        case 2:
-            return m2.get_angle();
-        default:
-            return NULL;
-    }
-}
-
-/**
- * Compute end-effector x-coordinate [m] for given motor angles [deg].
- */
-double ForwardKinematicsX(double theta_1, double theta_2)
-{
-    return kL1*cos(deg2rad(theta_1)) + kL2*cos(deg2rad(theta_2));
-}
-
-/**
- * Compute end-effector y-coordinate [m] for given motor angles [deg].
+ * Main
  */
-double ForwardKinematicsY(double theta_1, double theta_2)
-{
-    return kL1*sin(deg2rad(theta_1)) + kL2*sin(deg2rad(theta_2));
-}
-
-/**
- * Compute end-effector x-coordinate [m] for current motor angles.
- */
-double get_x()
-{
-    return ForwardKinematicsX(get_angle(1),get_angle(2));
-}
-
-/**
- * Compute end-effector y-coordinate [m] for current motor angles.
- */
-double get_y()
-{
-    return ForwardKinematicsY(get_angle(1),get_angle(2));
-}
-
-/**
- * Compute motor angle [deg] for motor i=1,2 for given end-effector x and y
- * coordinate [m]
- */
-double InverseKinematicsTheta(double x, double y, int i)
-{
-    // vector to end point
-    double r = sqrt(pow(x,2) + pow(y,2));
-    double dir_angle = rad2deg( atan2(y,x) );
-
-    // law of cosines for internal angles
-    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)) );
-
-    // absolute motor angles w.r.t x,y frame
-    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;
-    }
-}
-
-/**
- * Start robot:
- * - set reference position to current position s.t. no 'explosion'
- * - start motors
- * - reset controller
- */
-void Start()
-{
-    ui::serial.printf("! Starting robot.\r\n");
-    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 Stop()
-{
-    ui::serial.printf("! Stopping robot.\r\n");
-    ui::serial.printf("  - Stopping motors.\r\n");
-    m1.Stop();
-    m2.Stop();
-}
-
-/**
- * Control loop; negative feedback control on error with reference postion
- */
-void ControlLoop()
-{
-    m1.set_pwm(c1.Update( ref::theta_1 - get_angle(1) ));
-    m2.set_pwm(c2.Update( ref::theta_2 - get_angle(2) ));
-}
-
-/**
- * Set the motor angles to the values corresponding to the end-stops.
- * Only call this method when the robot is at its end-stops!
- */
-void SetCalibrationAngles()
+int main()
 {
-    m1.set_angle(kCalibAngleMotor1);
-    m2.set_angle(kCalibAngleMotor2);
-    is_calibrated = true;
-}
-
-} // end namespace robot
-
-
-
-// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
-
-/**
- * Reference implementation
- */
-namespace ref
-{
-
-/**
- * Get reference x-coordinate using reference angles and forward kinematics
- */
-double get_ref_x()
-{
-    return robot::ForwardKinematicsX(theta_1,theta_2);
-}
-
-/**
- * Get reference x-coordinate using reference angles and forward kinematics
- */
-double get_ref_y()
-{
-    return robot::ForwardKinematicsY(theta_1,theta_2);
-}
+    // Start with only red LED
+    ui::rgb_led[0] = LED_ON;
+    ui::rgb_led[1] = !LED_ON;
+    ui::rgb_led[2] = !LED_ON;
 
-/**
- * Update reference signal depending on robot state
- */
-void UpdateReference()
-{
-    switch(robot::state) {
-        case robot::CALIBRATION:
-            CalibrationReference();
-            break;
-        case robot::HOMING:
-            HomingReference();
-            break;
-        case robot::DEMO:
-            DemoReference();
-            break;
-    }
-}
+    // Setup serial comm
+    ui::serial.baud(115200);
+    ui::serial.printf("\r\n\r\n! STARTING MAIN LOOP.\r\n");
+    ui::serial.printf("  - Initializing...\r\n");
 
-/**
- * Generate reference profile to bring robotto calibration position;
- * at start-up the motor angles are '0' (relative encoders)
- */
-void CalibrationReference()
-{
-    // Check when motors can no longer move farther
-    bool is_ready_1 = (theta_1 - robot::get_angle(1) > kCalibrationError);
-    bool is_ready_2 = (theta_2 - robot::get_angle(2) > kCalibrationError);
-
-    // Turn off motor if end-stop reached
-    // Motor 2 _should_ arrive first
-    if (is_ready_1) {
-        robot::m1.Stop();
-    } else {
-        theta_1 += kCalibrationOmegaStep;
-    }
-
-    // Turn off motor if end-stop reached
-    if (is_ready_2) {
-        robot::m2.Stop();
-    } else {
-        theta_2 += kCalibrationOmegaStep;
-    }
-
-    if (is_ready_1 & is_ready_2) {
-        ui::serial.printf("  - Both motors at end-stop.\r\n");
-        robot::Stop();
-        // Set the calibration angles as current position
-        robot::SetCalibrationAngles();
-        robot::GoToNextState();
-    }
-}
+    // Ignore user interface while setup, ensure robot is off
+    ui::SwitchState(ui::IGNORE);
+    robot::SwitchState(robot::OFF);
 
-/**
- * Bring robot to home position: STARTING FROM CALIBRATION POSITION!!
- */
-void HomingReference()
-{
-    // First move motor 1
-    double dist_1 = kOriginTheta1 - theta_1;
-
-    if (abs(dist_1)<=kCalibrationOmegaStep) {
-        // Motor 1 ready
-        theta_1 = kOriginTheta1;
-
-        // Only move motor 2 after motor 1 at home
-        double dist_2 = kOriginTheta2 - theta_2;
-
-        if (abs(dist_2)<=kCalibrationOmegaStep) {
-            // motor 2 also ready
-            theta_2 = kOriginTheta2;
+    // Tickers: status, control and reference signal
+    Ticker tick_status;
+    Ticker tick_control_loop;
+    Ticker tick_reference;
 
-            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;
-    }
-}
-
-/**
- * Cycle through demo coordinates to display straight lines
- */
-void DemoReference()
-{
-    // Compare current setpoint and goal coord and slowly move linearly in the
-    // right direction (straight line 2D)
-
-    double x = robot::ForwardKinematicsX(theta_1,theta_2);
-    double y = robot::ForwardKinematicsY(theta_1,theta_2);
+    tick_status.attach(         &ui::StatusDisplay,     ui::kSampleTime);
+    tick_control_loop.attach(&robot::ControlLoop,    robot::kSampleTime);
+    tick_reference.attach(     &ref::UpdateReference,  ref::kSampleTime);
 
-    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));
+    // User input interrupt switches, pass to ui
+    InterruptIn sw2(SW2);
+    InterruptIn sw3(SW3);
 
-    // Compute next setpoint (x,y)
-    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;
+    sw2.rise(ui::InterruptSwitch2);
+    sw3.rise(ui::InterruptSwitch3);
 
-        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;
-    }
+    ui::serial.printf("  - Robot ready.\r\n");
+    ui::SwitchState(ui::STATE_SWITCHING);
 
-    // Compute motor angles
-    theta_1 = robot::InverseKinematicsTheta(x_new, y_new, 1);
-    theta_2 = robot::InverseKinematicsTheta(x_new, y_new, 2);
-}
-
-/**
- * Set current robot position as reference position.
- */
-void SetPositionAsReference()
-{
-    theta_1 = robot::get_angle(1);
-    theta_2 = robot::get_angle(2);
-}
-
-} // end namespace ref
+    // inf loop
+    while (true);
+}
\ No newline at end of file