Diff: main.cpp

Revision:

2:df0c6af898ac

Parent:

0:caa8ee3bd882

Child:

6:cddc73091ab5

--- a/main.cpp	Sun Nov 12 12:05:22 2017 +0000
+++ b/main.cpp	Sun Nov 12 14:06:23 2017 +0000
@@ -4,6 +4,9 @@
 // UI implementation
 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
 
+/**
+ * Display robot state using (flashing) LEDs
+ */
 void ui::StatusDisplay()
 {
     switch (robot::state) {
@@ -41,18 +44,15 @@
 }
 
 
-
+/**
+ * Switching UI state to a new state
+ */
 void ui::SwitchState(State new_state)
 {
-    ui::serial.printf("* Switching Input State to: %s\r\n",StateNames[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()
 {
@@ -79,7 +79,18 @@
 // Robot implementation
 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
 
+/**
+ * Switching ROBOT state to a new state
+ */
+void robot::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 robot::GoToNextState()
 {
     switch (state) {
@@ -99,9 +110,6 @@
         case READY:
             SwitchState(DEMO);
             break;
-        case DEMO:
-            SwitchState(MANUAL);
-            break;
         default:
             SwitchState(OFF);
             robot::is_calibrated = false;
@@ -109,11 +117,17 @@
     }
 }
 
+/**
+ * Check whether any of the motors is running
+ */
 bool robot::has_power()
 {
     return m1.has_power() or m2.has_power();
 }
 
+/**
+ * Get motor angle in [deg] for motor i=1,2
+ */
 double robot::get_angle(int i)
 {
     switch(i) {
@@ -126,37 +140,58 @@
     }
 }
 
+/**
+ * Compute end-effector x-coordinate [m] for given motor angles [deg].
+ */
 double robot::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].
+ */
 double robot::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 robot::get_x()
 {
     return ForwardKinematicsX(get_angle(1),get_angle(2));
 }
 
+/**
+ * Compute end-effector y-coordinate [m] for current motor angles.
+ */
 double robot::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 robot::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) );
-    
-    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)) );
-    
+
+    // 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;
@@ -164,33 +199,48 @@
     }
 }
 
+/**
+ * Start robot:
+ * - set reference position to current position s.t. no 'explosion'
+ * - start motors
+ * - reset controller
+ */
 void robot::Start()
 {
-    ui::serial.printf("  Setting current position as reference.\r\n");
+    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");
+    ui::serial.printf("  - Resetting controllers.\r\n");
     c1.Reset();
     c2.Reset();
 
-    ui::serial.printf("  Starting motors.\r\n");
+    ui::serial.printf("  - Starting motors.\r\n");
     m1.Start();
     m2.Start();
 }
 
 void robot::Stop()
 {
-    ui::serial.printf("  Stopping motors.\r\n");
+    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 robot::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 robot::SetCalibrationAngles()
 {
     m1.set_angle(kCalibAngleMotor1);
@@ -202,16 +252,25 @@
 // Reference implementation
 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
 
+/**
+ * Get reference x-coordinate using reference angles and forward kinematics
+ */
 double ref::get_ref_x()
 {
     return robot::ForwardKinematicsX(theta_1,theta_2);
 }
 
+/**
+ * Get reference x-coordinate using reference angles and forward kinematics
+ */
 double ref::get_ref_y()
 {
     return robot::ForwardKinematicsY(theta_1,theta_2);
 }
 
+/**
+ * Update reference signal depending on robot state
+ */
 void ref::UpdateReference()
 {
     switch(robot::state) {
@@ -227,18 +286,25 @@
     }
 }
 
+/**
+ * Generate reference profile to bring robotto calibration position;
+ * at start-up the motor angles are '0' (relative encoders)
+ */
 void ref::CalibrationReference()
 {
-    // Calibration process
+    // 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 {
@@ -246,66 +312,77 @@
     }
 
     if (is_ready_1 & is_ready_2) {
+        ui::serial.printf("  - Both motors at end-stop.\r\n");
         robot::Stop();
-        ui::serial.printf("+ Both motors at end-stop.\r\n");
+        // Set the calibration angles as current position
         robot::SetCalibrationAngles();
         robot::GoToNextState();
     }
 }
 
+/**
+ * Bring robot to home position: STARTING FROM CALIBRATION POSITION!!
+ */
 void ref::HomingReference()
 {
+    // First move motor 1
     double dist_1 = kOriginTheta1 - theta_1;
 
     if (abs(dist_1)<=kCalibrationOmegaStep) {
-        // motor 1 ready
+        // 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;
-            
-            ui::serial.printf("  Robot at home position.\r\n");
+
+            ui::serial.printf("  - Robot at home position.\r\n");
             robot::GoToNextState();
         } else {
-            // motor 2 still to move
+            // Motor 2 still to move
             theta_2 += dist_2 / abs(dist_2) * kCalibrationOmegaStep;
         }
 
 
     } else {
-        // motor 1 still to move
+        // Motor 1 still to move
         theta_1 += dist_1 / abs(dist_1) * kCalibrationOmegaStep;
     }
-
-
 }
 
+/**
+ * Cycle through demo coordinates to display straight lines
+ */
 void ref::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);
-    
+
     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));
-    
+
+    // 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);
+
+        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);
@@ -314,15 +391,17 @@
         x_new += delta_x / dist * kMaxStep;
         y_new += delta_y / dist * kMaxStep;
     }
-    
+
+    // 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 ref::SetPositionAsReference()
 {
     ref::theta_1 = robot::get_angle(1);
     ref::theta_2 = robot::get_angle(2);
-}
\ No newline at end of file
+}