Diff: main.cpp

Revision:

6:cddc73091ab5

Parent:

2:df0c6af898ac

Child:

7:b9a209f889f5

--- a/main.cpp	Mon Nov 13 09:11:42 2017 +0000
+++ b/main.cpp	Mon Nov 13 09:34:39 2017 +0000
@@ -1,13 +1,17 @@
 #include "main.h"
 
 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
-// UI implementation
-// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
+
+/**
+ * UI implementation
+ */
+namespace ui
+{
 
 /**
  * Display robot state using (flashing) LEDs
  */
-void ui::StatusDisplay()
+void StatusDisplay()
 {
     switch (robot::state) {
         case robot::OFF: // continuous red;
@@ -47,14 +51,14 @@
 /**
  * Switching UI state to a new state
  */
-void ui::SwitchState(State new_state)
+void SwitchState(State new_state)
 {
-    ui::serial.printf("* Switching INPUT State to: %s\r\n",StateNames[new_state]);
-    ui::state = new_state;
+    serial.printf("* Switching INPUT State to: %s\r\n",StateNames[new_state]);
+    state = new_state;
 }
 
 
-void ui::InterruptSwitch2()
+void InterruptSwitch2()
 {
     if (robot::has_power()) {
         robot::Stop();
@@ -63,7 +67,7 @@
     }
 }
 
-void ui::InterruptSwitch3()
+void InterruptSwitch3()
 {
     switch (state) {
         case STATE_SWITCHING:
@@ -72,17 +76,24 @@
     }
 }
 
+} // end namespace ui
+
 
 
 
 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
-// Robot implementation
-// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
+
+/**
+ * Robot implementation
+ */
+
+namespace robot
+{
 
 /**
  * Switching ROBOT state to a new state
  */
-void robot::SwitchState(State new_state)
+void SwitchState(State new_state)
 {
     ui::serial.printf("* Switching ROBOT State to: %s\r\n",StateNames[new_state]);
     state = new_state;
@@ -91,7 +102,7 @@
 /**
  * State flow // MANUAL state not implemented
  */
-void robot::GoToNextState()
+void GoToNextState()
 {
     switch (state) {
         case OFF:
@@ -112,7 +123,7 @@
             break;
         default:
             SwitchState(OFF);
-            robot::is_calibrated = false;
+            is_calibrated = false;
             Stop();
     }
 }
@@ -120,7 +131,7 @@
 /**
  * Check whether any of the motors is running
  */
-bool robot::has_power()
+bool has_power()
 {
     return m1.has_power() or m2.has_power();
 }
@@ -128,7 +139,7 @@
 /**
  * Get motor angle in [deg] for motor i=1,2
  */
-double robot::get_angle(int i)
+double get_angle(int i)
 {
     switch(i) {
         case 1:
@@ -143,7 +154,7 @@
 /**
  * Compute end-effector x-coordinate [m] for given motor angles [deg].
  */
-double robot::ForwardKinematicsX(double theta_1, double theta_2)
+double ForwardKinematicsX(double theta_1, double theta_2)
 {
     return kL1*cos(deg2rad(theta_1)) + kL2*cos(deg2rad(theta_2));
 }
@@ -151,7 +162,7 @@
 /**
  * Compute end-effector y-coordinate [m] for given motor angles [deg].
  */
-double robot::ForwardKinematicsY(double theta_1, double theta_2)
+double ForwardKinematicsY(double theta_1, double theta_2)
 {
     return kL1*sin(deg2rad(theta_1)) + kL2*sin(deg2rad(theta_2));
 }
@@ -159,7 +170,7 @@
 /**
  * Compute end-effector x-coordinate [m] for current motor angles.
  */
-double robot::get_x()
+double get_x()
 {
     return ForwardKinematicsX(get_angle(1),get_angle(2));
 }
@@ -167,7 +178,7 @@
 /**
  * Compute end-effector y-coordinate [m] for current motor angles.
  */
-double robot::get_y()
+double get_y()
 {
     return ForwardKinematicsY(get_angle(1),get_angle(2));
 }
@@ -176,7 +187,7 @@
  * Compute motor angle [deg] for motor i=1,2 for given end-effector x and y
  * coordinate [m]
  */
-double robot::InverseKinematicsTheta(double x, double y, int i)
+double InverseKinematicsTheta(double x, double y, int i)
 {
     // vector to end point
     double r = sqrt(pow(x,2) + pow(y,2));
@@ -205,7 +216,7 @@
  * - start motors
  * - reset controller
  */
-void robot::Start()
+void Start()
 {
     ui::serial.printf("! Starting robot.\r\n");
     ui::serial.printf("  - Setting current position as reference.\r\n");
@@ -220,7 +231,7 @@
     m2.Start();
 }
 
-void robot::Stop()
+void Stop()
 {
     ui::serial.printf("! Stopping robot.\r\n");
     ui::serial.printf("  - Stopping motors.\r\n");
@@ -231,7 +242,7 @@
 /**
  * Control loop; negative feedback control on error with reference postion
  */
-void robot::ControlLoop()
+void ControlLoop()
 {
     m1.set_pwm(c1.Update( ref::theta_1 - get_angle(1) ));
     m2.set_pwm(c2.Update( ref::theta_2 - get_angle(2) ));
@@ -241,21 +252,29 @@
  * 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 robot::SetCalibrationAngles()
+void SetCalibrationAngles()
 {
     m1.set_angle(kCalibAngleMotor1);
     m2.set_angle(kCalibAngleMotor2);
     is_calibrated = true;
 }
 
+} // end namespace robot
+
+
+
 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
-// Reference implementation
-// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
+
+/**
+ * Reference implementation
+ */
+namespace ref
+{
 
 /**
  * Get reference x-coordinate using reference angles and forward kinematics
  */
-double ref::get_ref_x()
+double get_ref_x()
 {
     return robot::ForwardKinematicsX(theta_1,theta_2);
 }
@@ -263,7 +282,7 @@
 /**
  * Get reference x-coordinate using reference angles and forward kinematics
  */
-double ref::get_ref_y()
+double get_ref_y()
 {
     return robot::ForwardKinematicsY(theta_1,theta_2);
 }
@@ -271,7 +290,7 @@
 /**
  * Update reference signal depending on robot state
  */
-void ref::UpdateReference()
+void UpdateReference()
 {
     switch(robot::state) {
         case robot::CALIBRATION:
@@ -290,7 +309,7 @@
  * Generate reference profile to bring robotto calibration position;
  * at start-up the motor angles are '0' (relative encoders)
  */
-void ref::CalibrationReference()
+void CalibrationReference()
 {
     // Check when motors can no longer move farther
     bool is_ready_1 = (theta_1 - robot::get_angle(1) > kCalibrationError);
@@ -323,7 +342,7 @@
 /**
  * Bring robot to home position: STARTING FROM CALIBRATION POSITION!!
  */
-void ref::HomingReference()
+void HomingReference()
 {
     // First move motor 1
     double dist_1 = kOriginTheta1 - theta_1;
@@ -356,7 +375,7 @@
 /**
  * Cycle through demo coordinates to display straight lines
  */
-void ref::DemoReference()
+void DemoReference()
 {
     // Compare current setpoint and goal coord and slowly move linearly in the
     // right direction (straight line 2D)
@@ -400,8 +419,10 @@
 /**
  * Set current robot position as reference position.
  */
-void ref::SetPositionAsReference()
+void SetPositionAsReference()
 {
-    ref::theta_1 = robot::get_angle(1);
-    ref::theta_2 = robot::get_angle(2);
+    theta_1 = robot::get_angle(1);
+    theta_2 = robot::get_angle(2);
 }
+
+} // end namespace ref