Revision 26:b16f1045108f, committed 2013-04-10

Comitter:

madcowswe

Date:

Wed Apr 10 02:01:51 2013 +0000

Parent:

25:50805ef8c499

Parent:

24:5cfc4789e00b

Child:

27:7cb3a21d9a2e

Child:

31:791739422122

Commit message:

Motion and motor works, but needs tuning

Changed in this revision

--- a/Processes/Kalman/Kalman.cpp	Tue Apr 09 20:37:59 2013 +0000
+++ b/Processes/Kalman/Kalman.cpp	Wed Apr 10 02:01:51 2013 +0000
@@ -6,7 +6,8 @@
 #include "math.h"
 #include "supportfuncs.h"
 #include "Encoder.h"
-//#include "globals.h"
+#include "globals.h"
+#include "Printing.h"
 
 #include "tvmet/Matrix.h"
 using namespace tvmet;
@@ -27,7 +28,7 @@
 Mutex statelock;
 
 float RawReadings[maxmeasure+1];
-float IRpahseOffset;
+float IRphaseOffset;
 
 bool Kalman_inited = 0;
 
@@ -88,15 +89,19 @@
         fromb0offset += constrainAngle(IR_Offsets[i] - IR_Offsets[0]);
     }
 
-    IRpahseOffset = constrainAngle(IR_Offsets[0] + fromb0offset/3);
+    IRphaseOffset = constrainAngle(IR_Offsets[0] + fromb0offset/3);
 
     //debug
-    printf("Offsets IR: %0.4f\r\n",IRpahseOffset*180/PI);
+    printf("Offsets IR: %0.4f\r\n",IRphaseOffset*180/PI);
 
     statelock.lock();
     X(0,0) = x_coor;
     X(1,0) = y_coor;
     X(2,0) = 0;
+    
+    P = 0.02*0.02, 0, 0,
+        0, 0.02*0.02, 0,
+        0, 0, 0.04;
     statelock.unlock();
     
     Kalman_inited = 1;
@@ -111,11 +116,11 @@
 }
 
 
-void predictloop(void const *dummy)
+void predictloop(void const*)
 {
 
-    //OLED4 = !ui.regid(0, 3);
-    //OLED4 = !ui.regid(1, 4);
+    OLED4 = !Printing::registerID(0, 3);
+    OLED4 = !Printing::registerID(1, 4);
 
     float lastleft = 0;
     float lastright = 0;
@@ -188,12 +193,12 @@
         P = F * P * trans(F) + Q;
 
         //printf("x: %f, y: %f, t: %f\r\n", X(0,0), X(1,0), X(2,0));
-        //Update UI
-        //float statecpy[] = {X(0,0), X(1,0), X(2,0)};
-        //ui.updateval(0, statecpy, 3);
+        //Update Printing
+        float statecpy[] = {X(0,0), X(1,0), X(2,0)};
+        Printing::updateval(0, statecpy, 3);
 
-        //float Pcpy[] = {P(0,0), P(0,1), P(1,0), P(1,1)};
-        //ui.updateval(1, Pcpy, 4);
+        float Pcpy[] = {P(0,0), P(0,1), P(1,0), P(1,1)};
+        Printing::updateval(1, Pcpy, 4);
 
         statelock.unlock();
     }
@@ -219,7 +224,7 @@
     } else {
 
         if (type >= IR0 && type <= IR2)
-            RawReadings[type] = value - IRpahseOffset;
+            RawReadings[type] = value - IRphaseOffset;
         else
             RawReadings[type] = value;
 
@@ -244,7 +249,7 @@
 
 }
 /*
-void Kalman::updateloop(void const *dummy)
+void Kalman::updateloop(void const*)
 {
 
     //sonar Y chanels

--- a/Processes/Kalman/Kalman.h	Tue Apr 09 20:37:59 2013 +0000
+++ b/Processes/Kalman/Kalman.h	Wed Apr 10 02:01:51 2013 +0000
@@ -11,8 +11,8 @@
 State getState();
 
 //Main loops (to be attached as a thread in main)
-void predictloop(void const *dummy);
-void updateloop(void const *dummy);
+void predictloop(void const*);
+void updateloop(void const*);
 
 void start_predict_ticker(Thread* predict_thread_ptr_in);
 
@@ -23,7 +23,7 @@
 void runupdate(measurement_t type, float value, float variance);
 
 extern float RawReadings[maxmeasure+1];
-extern float IRpahseOffset;
+extern float IRphaseOffset;
 
 extern bool Kalman_inited;
 

--- a/Processes/Motion/motion.cpp	Tue Apr 09 20:37:59 2013 +0000
+++ b/Processes/Motion/motion.cpp	Wed Apr 10 02:01:51 2013 +0000
@@ -6,6 +6,7 @@
 ////////////////////////////////////////////////////////////////////////////////
 
 #include "motion.h"
+#include "supportfuncs.h"
 
 namespace motion
 {
@@ -24,15 +25,17 @@
     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);
+    //printf("motion sees deltax: %f deltay %f\r\n", delta_x, delta_y);
     
-    float angle_err = atan2(delta_y, delta_x);
+    float distance_err = hypot(delta_x, delta_y);
+    
+    float angle_err = constrainAngle(atan2(delta_y, delta_x) - current_state.theta);
     
     
     // angular velocity controller
-    const float p_gain_av = 1.0; //TODO: tune
+    const float p_gain_av = 0.3; //TODO: tune
     
-    const float max_av = PI; // radians per sec //TODO: tune
+    const float max_av = 0.2*PI; // radians per sec //TODO: tune
     
     // angle error [-pi, pi]
     float angular_v = p_gain_av * angle_err;
@@ -45,9 +48,9 @@
     
     
     // forward velocity controller
-    const float p_gain_fv = 1.0; //TODO: tune
+    const float p_gain_fv = 0.3; //TODO: tune
     
-    float max_fv = 1.0; // meters per sec //TODO: tune
+    float max_fv = 0.2; // meters per sec //TODO: tune
     const float angle_envelope_exponent = 8.0; //TODO: tune
     
     // control, distance_err in meters
@@ -61,10 +64,12 @@
         forward_v = max_fv;
     else if (forward_v < -max_fv)
         forward_v = -max_fv;
+        
+    //printf("fwd: %f, omega: %f\r\n", forward_v, angular_v);
     
     //TODO: put into motor control
     MotorControl::set_fwdcmd(forward_v);
-    MotorControl::set_thetacmd(angular_v);
+    MotorControl::set_omegacmd(angular_v);
 }
 
 } //namespace
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Processes/MotorControl/MotorControl.cpp	Wed Apr 10 02:01:51 2013 +0000
@@ -0,0 +1,53 @@
+
+#include "MainMotor.h"
+#include "Encoder.h"
+#include "globals.h"
+#include <algorithm>
+
+namespace MotorControl
+{
+
+float fwdcmd = 0;
+float omegacmd = 0;
+
+void motor_control_isr()
+{
+
+    MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B);
+
+    float testspeed = 0.2;
+    float Pgain = 10;
+    static float lastT = SystemTime.read();
+    static float lastright = right_encoder.getTicks() * ENCODER_M_PER_TICK;
+    static float lastleft = left_encoder.getTicks() * ENCODER_M_PER_TICK;
+
+    static float thetafiltstate = 0;
+    static float fwdfiltstate = 0;
+
+    float currright = right_encoder.getTicks() * ENCODER_M_PER_TICK;
+    float dright = currright - lastright;
+    lastright = currright;
+
+    float currleft = left_encoder.getTicks() * ENCODER_M_PER_TICK;
+    float dleft = currleft - lastleft;
+    lastleft = currleft;
+
+    float currtime = SystemTime.read();
+    float dt = currtime - lastT;
+    lastT = currtime;
+
+    thetafiltstate = MOTORCONTROLLER_FILTER_K * thetafiltstate + (1-MOTORCONTROLLER_FILTER_K) * ((dright-dleft)/(dt*ENCODER_WHEELBASE));
+    fwdfiltstate = MOTORCONTROLLER_FILTER_K * fwdfiltstate + (1-MOTORCONTROLLER_FILTER_K) * ((dright+dleft)/(2.0f*dt));
+
+    float errfwd = fwdfiltstate - fwdcmd;
+    float errtheta = thetafiltstate - omegacmd;
+
+    float errleft = errfwd - (errtheta*ENCODER_WHEELBASE/2.0f);
+    float errright = errfwd + (errtheta*ENCODER_WHEELBASE/2.0f);
+
+    mleft(max(min(errleft*Pgain, MOTOR_MAX_POWER), -MOTOR_MAX_POWER));
+    mright(max(min(errright*Pgain, MOTOR_MAX_POWER), -MOTOR_MAX_POWER));
+
+}
+
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Processes/MotorControl/MotorControl.h	Wed Apr 10 02:01:51 2013 +0000
@@ -0,0 +1,22 @@
+
+#ifndef MOTOR_CONTROL_H
+#define MOTOR_CONTROL_H
+
+namespace MotorControl{
+    
+    extern float fwdcmd;
+    extern float omegacmd;
+    
+    inline void set_fwdcmd(float infwdcmd){
+        fwdcmd = infwdcmd;
+    }
+    
+    inline void set_omegacmd(float inomega){
+        omegacmd = inomega;
+    }
+    
+    void motor_control_isr();
+    
+}
+
+#endif //MOTOR_CONTROL_H
\ No newline at end of file

--- a/Processes/Printing/Printing.cpp	Tue Apr 09 20:37:59 2013 +0000
+++ b/Processes/Printing/Printing.cpp	Wed Apr 10 02:01:51 2013 +0000
@@ -1,17 +1,21 @@
 #include "Printing.h"
+#include <iostream>
+
+namespace Printing {
+
 #ifdef PRINTINGOFF
-void printingThread(void const*){Thread::wait(osWaitForever);}
+void printingloop(void const*){Thread::wait(osWaitForever);}
 bool registerID(char, size_t){return true;}
 bool unregisterID(char) {return true;}
 bool updateval(char, float*, size_t){return true;}
 bool updateval(char id, float value){return true;}
 #else
-#include <iostream>
+
 using namespace std;
 
-size_t idlist[NUMIDS]; // Stores length of buffer 0 => unassigned
-float* buffarr[NUMIDS];
-volatile unsigned int newdataflags;
+size_t idlist[NUMIDS] = {0}; // Stores length of buffer 0 => unassigned
+float* buffarr[NUMIDS] = {0};
+volatile unsigned int newdataflags = 0;
 
 bool registerID(char id, size_t length) {   
     if (id < NUMIDS && !idlist[id]) {//check if the id is already taken
@@ -52,16 +56,16 @@
         return false;
 }
 
-void printingThread(void const*){
-    newdataflags = 0;
-    for (int i = 0; i < NUMIDS; i++) {
-        idlist[i] = 0;
-        buffarr[i] = 0;
-    }
+void printingloop(void const*){
 
-
-    Thread::wait(3500);
-    while(true){   
+    Serial pc(USBTX, USBRX);
+    pc.baud(115200);
+    
+    char sync[] = "ABCD";
+    cout.write(sync, 4);
+    cout << std::endl;
+    
+    while(true){
         // Send number of packets
         char numtosend = 0;
         for (unsigned int v = newdataflags; v; numtosend++){v &= v - 1;}        
@@ -79,7 +83,9 @@
         Thread::wait(200);
     }
 }
-#endif
 
 
+#endif //end PRINTINGOFF
+
+} //end namespace
  

--- a/Processes/Printing/Printing.h	Tue Apr 09 20:37:59 2013 +0000
+++ b/Processes/Printing/Printing.h	Wed Apr 10 02:01:51 2013 +0000
@@ -1,16 +1,22 @@
 
 // Eurobot13 Printing.h
 
+//#define PRINTINGOFF
+
 #include "mbed.h"
 #include "rtos.h"
 
+namespace Printing {
+
 const size_t NUMIDS = sizeof(unsigned int)*8;
 
 //Function to start in Thread
-void printingThread(void const*); //
+void printingloop(void const*); //
 
 //Functions to use 
 bool registerID(char id, size_t length);
 bool unregisterID(char id);
 bool updateval(char id, float* buffer, size_t length);
 bool updateval(char id, float value);
+
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/globals.cpp	Wed Apr 10 02:01:51 2013 +0000
@@ -0,0 +1,7 @@
+
+#include "globals.h"
+
+//Store global objects here
+pos beaconpos[] = {{0,1}, {3,0}, {3,2}};
+Timer SystemTime;
+Waypoint* AI::current_waypoint;
\ No newline at end of file

--- a/globals.h	Tue Apr 09 20:37:59 2013 +0000
+++ b/globals.h	Wed Apr 10 02:01:51 2013 +0000
@@ -18,6 +18,11 @@
 const float xyvarpertime = 0.0005; //(very poorly) accounts for hitting things
 const float angvarpertime = 0.001;
 
+extern Timer SystemTime;
+
+const float MOTORCONTROLLER_FILTER_K = 0.5;// TODO: tune this
+const float MOTOR_MAX_POWER = 0.4f;
+
 /*
 PINOUT Sensors
 5:  RF:SDI
@@ -60,15 +65,15 @@
 
 const PinName P_COLOR_SENSOR_IN = p20;
 
-const PinName P_MOT_RIGHT_A     = p21;
-const PinName P_MOT_RIGHT_B     = p22;
-const PinName P_MOT_LEFT_A      = p23;
-const PinName P_MOT_LEFT_B      = p24;
+const PinName P_MOT_LEFT_A     = p21;
+const PinName P_MOT_LEFT_B     = p22;
+const PinName P_MOT_RIGHT_A      = p23;
+const PinName P_MOT_RIGHT_B      = p24;
 
-const PinName P_ENC_RIGHT_A     = p28;
-const PinName P_ENC_RIGHT_B     = p27;
-const PinName P_ENC_LEFT_A      = p25;
-const PinName P_ENC_LEFT_B      = p26;
+const PinName P_ENC_RIGHT_A     = p26;
+const PinName P_ENC_RIGHT_B     = p25;
+const PinName P_ENC_LEFT_A      = p27;
+const PinName P_ENC_LEFT_B      = p28;
 
 const PinName P_COLOR_SENSOR_RED = p29;
 const PinName P_COLOR_SENSOR_BLUE = p30;
@@ -112,7 +117,7 @@
 //TODO: hack, move to AI layer
 namespace AI
 {
-    Waypoint *current_waypoint; 
+    extern Waypoint* current_waypoint; 
 }
 
 

--- a/main.cpp	Tue Apr 09 20:37:59 2013 +0000
+++ b/main.cpp	Wed Apr 10 02:01:51 2013 +0000
@@ -2,17 +2,16 @@
 #include "Kalman.h"
 #include "mbed.h"
 #include "rtos.h"
-#include "Actuators/Arms/Arm.h"
-#include "Actuators/MainMotors/MainMotor.h"
-#include "Sensors/Encoders/Encoder.h"
-#include "Sensors/Colour/Colour.h"
-#include "Sensors/CakeSensor/CakeSensor.h"
-#include "Processes/Printing/Printing.h"
+#include "Arm.h"
+#include "MainMotor.h"
+#include "Encoder.h"
+#include "Colour.h"
+#include "CakeSensor.h"
+#include "Printing.h"
 #include "coprocserial.h"
 #include <algorithm>
 #include "motion.h"
-
-pos beaconpos[] = {{0,1}, {3,0}, {3,2}};
+#include "MotorControl.h"
 
 void motortest();
 void encodertest();
@@ -51,13 +50,20 @@
     
      /*
     DigitalOut l1(LED1);
-    Thread p(printingThread,        NULL,   osPriorityNormal,   2048);
+    Thread p(Printing::printingloop,        NULL,   osPriorityNormal,   2048);
     l1=1;
     Thread a(printingtestthread,    NULL,   osPriorityNormal,   1024);
     Thread b(printingtestthread2,   NULL,   osPriorityNormal,   1024);
     Thread::wait(osWaitForever);
     */
+    
+    SystemTime.start();
+    
+    Serial pc(USBTX, USBRX);
+    pc.baud(115200);
+    
     using AI::current_waypoint;
+    
     current_waypoint = new Waypoint;
     current_waypoint->x = 0.5;
     current_waypoint->y = 0.7;
@@ -75,13 +81,19 @@
     
     Kalman::start_predict_ticker(&predictthread);
     
+    Ticker motorcontroltestticker;
+    motorcontroltestticker.attach(MotorControl::motor_control_isr, 0.05);
+    
     // motion layer periodic callback
     RtosTimer motion_timer(motion::motionlayer, osTimerPeriodic);
     motion_timer.start(50);
-    
+
+   
+    Thread::wait(3500);
+    Thread printingThread(Printing::printingloop, NULL, osPriorityLow, 2048);
+
     Thread::wait(osWaitForever);
-    //feedbacktest();
-    
+
 }
 
 #include <cstdlib>
@@ -90,12 +102,12 @@
 void printingtestthread(void const*){
     const char ID = 1;
     float buffer[3] = {ID};
-    registerID(ID,sizeof(buffer)/sizeof(buffer[0]));
+    Printing::registerID(ID,sizeof(buffer)/sizeof(buffer[0]));
     while (true){
         for(size_t i = 1; i != sizeof(buffer)/sizeof(buffer[0]); ++i){
             buffer[i] = ID ;
         }
-        updateval(ID, buffer, sizeof(buffer)/sizeof(buffer[0]));
+        Printing::updateval(ID, buffer, sizeof(buffer)/sizeof(buffer[0]));
         Thread::wait(200);
     }
 }
@@ -103,16 +115,17 @@
 void printingtestthread2(void const*){
     const char ID = 2;
     float buffer[5] = {ID};
-    registerID(ID,sizeof(buffer)/sizeof(buffer[0]));
+    Printing::registerID(ID,sizeof(buffer)/sizeof(buffer[0]));
     while (true){
         for(size_t i = 1; i != sizeof(buffer)/sizeof(buffer[0]); ++i){
             buffer[i] = ID;
         }
-        updateval(ID, buffer, sizeof(buffer)/sizeof(buffer[0]));
+        Printing::updateval(ID, buffer, sizeof(buffer)/sizeof(buffer[0]));
         Thread::wait(500);
     }
 }
 
+
 /*
 void feedbacktest(){
     //Encoder Eright(P_ENC_RIGHT_A, P_ENC_RIGHT_B), Eleft(P_ENC_LEFT_A, P_ENC_LEFT_B);
@@ -136,6 +149,7 @@
     }
 }
 */
+
 void cakesensortest(){
     wait(1);
     printf("cakesensortest");