Revision 25:50805ef8c499, committed 2013-04-09

Comitter:

rsavitski

Date:

Tue Apr 09 20:37:59 2013 +0000

Parent:

20:70d651156779

Child:

26:b16f1045108f

Commit message:

Motion control branch

Changed in this revision

--- a/Processes/Kalman/Kalman.h	Tue Apr 09 15:33:36 2013 +0000
+++ b/Processes/Kalman/Kalman.h	Tue Apr 09 20:37:59 2013 +0000
@@ -1,18 +1,12 @@
 #ifndef KALMAN_H
 #define KALMAN_H
 
-//#include "globals.h"
+#include "globals.h"
 #include "rtos.h"
 
 namespace Kalman
 {
 
-typedef struct State {
-    float x;
-    float y;
-    float theta;
-} State ;
-
 //Accessor function to get the state as one consistent struct
 State getState();
 

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Processes/Motion/motion.cpp	Tue Apr 09 20:37:59 2013 +0000
@@ -0,0 +1,70 @@
+////////////////////////////////////////////////////////////////////////////////
+// Motion control unit
+////////////////////////////////////////////////////////////////////////////////
+// Takes current state of the robot and target waypoint,
+// calculates desired forward and angular velocities and requests those from the motor control layer.
+////////////////////////////////////////////////////////////////////////////////
+
+#include "motion.h"
+
+namespace motion
+{
+
+void motionlayer(void const *dummy)
+{
+    //TODO: current_waypoint global in AI layer
+    //TODO: threshold for deciding that the waypoint has been achieved
+    
+    // get target waypoint from AI
+    Waypoint target_waypoint = *AI::current_waypoint;
+    
+    // get current state from Kalman
+    State current_state = Kalman::getState();
+    
+    float delta_x = target_waypoint.x - current_state.x;
+    float delta_y = target_waypoint.y - current_state.y;
+    
+    float distance_err = sqrt(delta_x*delta_x + delta_y*delta_y);
+    
+    float angle_err = atan2(delta_y, delta_x);
+    
+    
+    // angular velocity controller
+    const float p_gain_av = 1.0; //TODO: tune
+    
+    const float max_av = PI; // radians per sec //TODO: tune
+    
+    // angle error [-pi, pi]
+    float angular_v = p_gain_av * angle_err;
+    
+    // constrain range
+    if (angular_v > max_av)
+        angular_v = max_av;
+    else if (angular_v < -max_av)
+        angular_v = -max_av;
+    
+    
+    // forward velocity controller
+    const float p_gain_fv = 1.0; //TODO: tune
+    
+    float max_fv = 1.0; // meters per sec //TODO: tune
+    const float angle_envelope_exponent = 8.0; //TODO: tune
+    
+    // control, distance_err in meters
+    float forward_v = p_gain_fv * distance_err;
+    
+    // control the forward velocity envelope based on angular error
+    max_fv = max_fv * pow(cos(angle_err/2), angle_envelope_exponent);
+    
+    // constrain range
+    if (forward_v > max_fv)
+        forward_v = max_fv;
+    else if (forward_v < -max_fv)
+        forward_v = -max_fv;
+    
+    //TODO: put into motor control
+    MotorControl::set_fwdcmd(forward_v);
+    MotorControl::set_thetacmd(angular_v);
+}
+
+} //namespace
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Processes/Motion/motion.h	Tue Apr 09 20:37:59 2013 +0000
@@ -0,0 +1,17 @@
+#ifndef EUROBOT_PROCESSES_MOTION_MOTION_H_
+#define EUROBOT_PROCESSES_MOTION_MOTION_H_
+
+#include "globals.h"
+#include "rtos.h"
+#include "math.h"
+#include "Kalman.h"
+#include "MotorControl.h"
+
+namespace motion
+{
+
+void motionlayer(void const *dummy);
+
+}
+
+#endif //EUROBOT_PROCESSES_MOTION_MOTION_H_
\ No newline at end of file

--- a/globals.h	Tue Apr 09 15:33:36 2013 +0000
+++ b/globals.h	Tue Apr 09 20:37:59 2013 +0000
@@ -93,4 +93,27 @@
 
 const float PI = 3.14159265359;
 
+typedef struct State 
+{
+    float x;
+    float y;
+    float theta;
+} State;
+
+typedef struct Waypoint
+{
+    float x;
+    float y;
+    float theta;
+    float pos_threshold;
+    float angle_threshold;
+} Waypoint;
+
+//TODO: hack, move to AI layer
+namespace AI
+{
+    Waypoint *current_waypoint; 
+}
+
+
 #endif //GLOBALS_H
\ No newline at end of file

--- a/main.cpp	Tue Apr 09 15:33:36 2013 +0000
+++ b/main.cpp	Tue Apr 09 20:37:59 2013 +0000
@@ -10,6 +10,7 @@
 #include "Processes/Printing/Printing.h"
 #include "coprocserial.h"
 #include <algorithm>
+#include "motion.h"
 
 pos beaconpos[] = {{0,1}, {3,0}, {3,2}};
 
@@ -56,7 +57,13 @@
     Thread b(printingtestthread2,   NULL,   osPriorityNormal,   1024);
     Thread::wait(osWaitForever);
     */
-    
+    using AI::current_waypoint;
+    current_waypoint = new Waypoint;
+    current_waypoint->x = 0.5;
+    current_waypoint->y = 0.7;
+    current_waypoint->theta = 0.0;
+    current_waypoint->pos_threshold = 0.02;
+    current_waypoint->angle_threshold = 0.09;
     
     InitSerial();
     //while(1)
@@ -67,8 +74,13 @@
     Thread predictthread(Kalman::predictloop, NULL, osPriorityNormal, 2084);//512); //temp 2k
     
     Kalman::start_predict_ticker(&predictthread);
-    //Thread::wait(osWaitForever);
-    feedbacktest();
+    
+    // motion layer periodic callback
+    RtosTimer motion_timer(motion::motionlayer, osTimerPeriodic);
+    motion_timer.start(50);
+    
+    Thread::wait(osWaitForever);
+    //feedbacktest();
     
 }
 
@@ -101,7 +113,7 @@
     }
 }
 
-
+/*
 void feedbacktest(){
     //Encoder Eright(P_ENC_RIGHT_A, P_ENC_RIGHT_B), Eleft(P_ENC_LEFT_A, P_ENC_LEFT_B);
     MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B);
@@ -123,7 +135,7 @@
         mright(max(min(errright*Pgain, 0.4f), -0.4f));
     }
 }
-
+*/
 void cakesensortest(){
     wait(1);
     printf("cakesensortest");