Revision 16:a1ede21a963b, committed 2016-01-07

Comitter:

IceTeam

Date:

Thu Jan 07 13:56:38 2016 +0100

Parent:

15:0ea6237f4e92

Parent:

14:a394e27b8cb2

Child:

17:e32b4b54fc04

Commit message:

Fusion

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Asserv_Plan_B/Scratch.h	Thu Jan 07 13:56:38 2016 +0100
@@ -0,0 +1,20 @@
+/*float erreur_distance_g = distance_g-(m_odometry.getDistLeft()-memo_g); //- distance parcourue par la roue gauche depuis l'état 2
+float erreur_distance_d = distance_d-(m_odometry.getDistRight()-memo_d);
+cmd_g = erreur_distance_g*Kp_distance;
+cmd_d = erreur_distance_d*Kp_distance;
+m_motorL.setSpeed(cmd_g);
+m_motorR.setSpeed(cmd_d);*/
+
+/*void aserv_planB::control_speed()
+{
+    vitesse_d = m_odometry.getVitRight();
+    vitesse_g = m_odometry.getVitLeft();
+
+    erreur_g = consigne_g - vitesse_g;
+    cmd_g = erreur_g*Kp;
+    erreur_d = consigne_d - vitesse_d;
+    cmd_d = erreur_d*Kp;
+
+    m_motorL.setSpeed(cmd_g);
+    m_motorR.setSpeed(cmd_d);
+}*/
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Asserv_Plan_B/planB.cpp	Thu Jan 07 13:56:38 2016 +0100
@@ -0,0 +1,199 @@
+#include "planB.h"
+#include "defines.h"
+
+extern Serial logger;
+
+aserv_planB::aserv_planB(Odometry2 &odometry,Motor &motorL,Motor &motorR) : m_odometry(odometry), m_motorL(motorL), m_motorR(motorR)
+{
+    limite = 0.55;
+    lim_max = 0.30;
+    lim_min = 0.1995;
+    cmd = 0;
+    cmd_g = 0;
+    cmd_d = 0;
+    
+    somme_erreur_theta = 0;
+    delta_erreur_theta = 0;
+    erreur_precedente_theta = 0;
+    
+    somme_erreur_distance = 0;
+    delta_erreur_distance = 0;
+    erreur_precedente_distance = 0;
+    distanceGoal = 0;
+    distance = 0;
+    
+    Kp_angle = 3.5; //Fixed à 3.0 pour 180 deg
+    Ki_angle = 0.0;
+    Kd_angle = 0.2;
+    
+    Kp_distance = 0.0041;
+    Ki_distance = 0.0001;//0.000001
+    Kd_distance = 0.01;//0.05
+    
+    N = 0;
+    arrived = false;
+    squip = false;
+    state = 0; // Etat ou l'on ne fait rien
+}
+
+void aserv_planB::setGoal(float x, float y, float phi)
+{
+    arrived = false;
+    m_goalX = x;
+    m_goalY = y;
+    m_goalPhi = phi;
+    distanceGoal = sqrt(carre(m_goalX-m_odometry.getX())+carre(m_goalY-m_odometry.getY()));
+    state = 1; // Etat de rotation 1
+    Kd_distance = 0.01;
+    N = 0;
+}
+
+void aserv_planB::stop(void)
+{
+    m_motorL.setSpeed(0);
+    m_motorR.setSpeed(0);
+    state = 0;
+}
+
+void aserv_planB::setGoal(float x, float y)
+{
+    squip = true;
+    setGoal(x, y, 0);
+}
+
+void aserv_planB::update(float dt)
+{
+    thetaGoal = atan2(m_goalY-m_odometry.getY(),m_goalX-m_odometry.getX());
+    float erreur_theta = thetaGoal-m_odometry.getTheta();
+    
+    float erreur_distance = sqrt(carre(m_goalX-m_odometry.getX())+carre(m_goalY-m_odometry.getY()));
+    
+    delta_erreur_theta = erreur_theta - erreur_precedente_theta;
+    erreur_precedente_theta = erreur_theta;
+    somme_erreur_theta += erreur_theta;
+    
+    delta_erreur_distance = erreur_distance - erreur_precedente_distance;
+    erreur_precedente_distance = erreur_distance;
+    somme_erreur_distance += erreur_distance;
+    
+    if(erreur_theta <= PI) erreur_theta += 2.0f*PI;
+    if(erreur_theta >= PI) erreur_theta -= 2.0f*PI;
+
+    // Etat 1 : Angle theta pour viser dans la direction du point M(x,y)
+    if(state == 1)
+    {
+        //logger.printf("%.2f\r\n", erreur_theta*180/PI);
+        cmd = erreur_theta*Kp_angle + delta_erreur_theta*Kd_angle + somme_erreur_theta*Ki_angle;
+        
+        if(cmd > limite) cmd = limite;
+        else if(cmd < -limite) cmd = -limite;
+        
+        m_motorL.setSpeed(-cmd);
+        m_motorR.setSpeed(cmd);
+        
+        if(abs(erreur_theta) < 0.05f) N++;
+        else N = 0;
+        if(N > 5)
+        {
+            m_motorL.setSpeed(0);
+            m_motorR.setSpeed(0);
+            state = 2;
+            //logger.printf("Erreur theta : %.2f\r\n", erreur_theta*180/PI);
+            somme_erreur_theta = 0;
+        }
+    }
+
+    // Etat 2 : Parcours du robot jusqu'au point M(x,y)
+    if(state == 2) 
+    {
+        //Source d'erreurs et de ralentissements
+        /*if(delta_erreur_distance > 0) // On a dépassé le point
+        {
+            state = 1;
+            return;
+        }*/
+        
+        if(abs(erreur_distance) > 55.0f) somme_erreur_distance = 0;
+        
+        cmd_g = erreur_distance*Kp_distance + somme_erreur_distance*Ki_distance + delta_erreur_distance*Kd_distance - (erreur_theta*Kp_angle + delta_erreur_theta*Kd_angle + somme_erreur_theta*Ki_angle);
+        cmd_d = erreur_distance*Kp_distance + somme_erreur_distance*Ki_distance + delta_erreur_distance*Kd_distance + erreur_theta*Kp_angle + delta_erreur_theta*Kd_angle + somme_erreur_theta*Ki_angle;
+        
+        
+        if(abs(erreur_distance) > 55.0f)
+        {
+            if(cmd_g > limite) cmd_g = limite;
+            else if(cmd_g < -limite) cmd_g = -limite;
+            
+            if(cmd_d > limite) cmd_d = limite;
+            else if(cmd_d < -limite) cmd_d = -limite;
+        }
+        else
+        {
+            Kd_distance = 0.01;
+            if(cmd_g > lim_max) cmd_g = lim_max;
+            else if(cmd_g < -lim_max) cmd_g = -lim_max;
+            
+            if(cmd_d > lim_max) cmd_d = lim_max;
+            else if(cmd_d < -lim_max) cmd_d = -lim_max;
+        }
+        
+        if(cmd_g > 0.01f && cmd_g < lim_min) cmd_g = lim_min;
+        if(cmd_g < -0.01f && cmd_g > -lim_min) cmd_g = -lim_min;
+        
+        if(cmd_d > 0.01f && cmd_d < lim_min) cmd_d = lim_min;
+        if(cmd_d < -0.01f && cmd_d > -lim_min) cmd_d = -lim_min;
+            
+        m_motorL.setSpeed(cmd_g);
+        m_motorR.setSpeed(cmd_d);
+        
+        if(abs(erreur_distance) < 5.0f) N++;
+        else N = 0;
+        if(N > 10)
+        {
+            delta_erreur_theta = 0;
+            erreur_precedente_theta = 0;
+            somme_erreur_theta = 0;
+            erreur_theta = 0;
+            m_motorL.setSpeed(0);
+            m_motorR.setSpeed(0);
+            if(squip == true) 
+            {
+                arrived = true;
+                squip = false;
+                state = 0;
+            }
+            else state = 3;
+            //logger.printf("Erreur distance : %.2f, Arrived : %d, Etat = %d\r\n", erreur_distance, arrived, (int)state);
+        }
+    }
+
+    // Etat 3 : Placement au bon angle Phi souhaité au point M(x,y)
+    if(state == 3)
+    {
+        erreur_theta = m_goalPhi-m_odometry.getTheta();
+        
+        if(erreur_theta <= PI) erreur_theta += 2.0f*PI;
+        if(erreur_theta >= PI) erreur_theta -= 2.0f*PI;
+        
+        cmd = erreur_theta*Kp_angle;
+        
+        if(cmd > limite) cmd = limite;
+        else if(cmd < -limite) cmd = -limite;
+        
+        m_motorL.setSpeed(-cmd);
+        m_motorR.setSpeed(cmd);
+        
+        if(abs(erreur_theta)< 0.05) N++;
+        else N = 0;
+        if(N > 10)
+        {
+            //logger.printf("Erreur theta : %.2f\r\n", erreur_theta*180/PI);
+            somme_erreur_theta = 0;
+            arrived = true;
+            squip = false;
+            state = 0;
+            m_motorL.setSpeed(0);
+            m_motorR.setSpeed(0);
+        }
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Asserv_Plan_B/planB.h	Thu Jan 07 13:56:38 2016 +0100
@@ -0,0 +1,48 @@
+#ifndef PLANB_H
+#define PLANB_H
+
+#include "mbed.h"
+#include "Odometry2.h"
+#include "Motor.h"
+
+class aserv_planB
+{
+public:
+    aserv_planB(Odometry2 &odometry,Motor &motorL,Motor &motorR);
+    void update(float dt);
+    void control_speed();
+    void setGoal(float x, float y, float theta);
+    void setGoal(float x, float y);
+    void stop(void);
+    bool isArrived(void) {return arrived;}
+    float carre(float x) {return x*x;}
+    float Kp_angle, Kd_angle, Ki_angle;
+    float Kp_distance, Ki_distance, Kd_distance;
+
+private:
+    Odometry2 &m_odometry;
+    Motor &m_motorL;
+    Motor &m_motorR;
+    
+    float cmd;
+    float cmd_g, cmd_d;
+    float limite;
+    float lim_min, lim_max;
+    
+    float somme_erreur_theta, somme_erreur_distance;
+    float delta_erreur_theta, delta_erreur_distance;
+    float erreur_precedente_theta, erreur_precedente_distance;
+    float distanceGoal, distance;
+    float thetaGoal;
+    
+    float m_goalX, m_goalY, m_goalPhi;
+    
+    bool arrived, squip;
+    int N;
+    
+    char state;
+    //char etat_angle;
+};
+
+
+#endif

--- a/Map/Map.cpp	Tue Jan 05 15:55:56 2016 +0100
+++ b/Map/Map.cpp	Thu Jan 07 13:56:38 2016 +0100
@@ -26,7 +26,7 @@
 
 void Map::build()
 {
-    obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_MG,-100,-100,2100,0));// MG
+    /*obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_MG,-100,-100,2100,0));// MG
     obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_MB,2100,-100,2000,3100));// MB
     obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_MH,-100,-100,0,3100));// MH
     obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_MD,2100,3000,-100,3100));// MD
@@ -68,8 +68,8 @@
     obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P12,1355,3000-870,30));// P12
     obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P13,1750,3000-90,30));// P13
     obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P14,1850,3000-90,30));// P14
-    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P15,1770,3000-1100,30));// P15
-    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P16,1400,3000-1300,30));// P16
+    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P15,1770,3000-1100,30));// P15*/
+    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P16,1000,1000,30));// P16
 }
 
 int Map::getHeight(float x, float y)

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Map/defines.h	Thu Jan 07 13:56:38 2016 +0100
@@ -0,0 +1,118 @@
+#ifndef DEFINE_H
+#define DEFINE_H
+
+#define PLAN_B
+//#define OUT_USB
+
+
+enum ID
+{
+    IDO_MG,
+    IDO_MH,
+    IDO_MD,
+    IDO_MB,
+    IDO_M1,
+    IDO_M2,
+    IDO_M3,
+    IDO_M4,
+    IDO_M5,
+    IDO_M6,
+    IDO_D1,
+    IDO_D2,
+    IDO_D3,
+    IDO_D4,
+    IDO_E,
+    IDO_S,
+    IDO_PC1,
+    IDO_PC2,
+    IDO_PC3,
+    IDO_PC4,
+    IDO_PC5,
+    IDO_P1,
+    IDO_P2,
+    IDO_P3,
+    IDO_P4,
+    IDO_P5,
+    IDO_P6,
+    IDO_P7,
+    IDO_P8,
+    IDO_P9,
+    IDO_P10,
+    IDO_P11,
+    IDO_P12,
+    IDO_P13,
+    IDO_P14,
+    IDO_P15,
+    IDO_P16,
+    IDO_DEPOT_PC,
+    IDO_DEPOT_P
+};
+
+#define ROBOTRADIUS 190
+
+#define MAXPOINT 8000
+
+// ----- Loggeur ----- //
+
+#ifdef OUT_USB
+    #define OUT_TX USBTX
+    #define OUT_RX USBRX
+#else
+    #define OUT_TX PA_11
+    #define OUT_RX PA_12
+#endif
+
+// ----- Moteurs ----- //
+
+#define PWM_MOT1 PB_13
+#define PWM_MOT2 PB_14
+#define PWM_MOT3 PB_15
+
+#define DIR_MOT1 PC_9
+#define DIR_MOT2 PB_8
+#define DIR_MOT3 PB_9
+
+// ----- Odometrie ----- //
+
+#define ODO_G_B PA_10
+#define ODO_G_A PB_3
+
+#define ODO_D_B PB_5
+#define ODO_D_A PB_4
+
+#define PI 3.14159f
+
+// ----- Boutons ----- //
+
+#define LED_DESSUS PH_1
+#define BP_DESSUS PC_8
+#define TIRETTE_DESSUS PC_6
+#define COULEUR_DESSUS PC_5
+
+#define COULEUR_JAUNE 0
+#define COULEUR_VERTE 1
+
+// ----- AX12 ----- //
+
+#define AX12_TX PA_9
+#define AX12_RX NC
+
+#define MAX_TORQUE 300
+
+#define BRASG_OUVERT 60
+#define BRASG_FERME 155
+#define BRASD_OUVERT 240
+#define BRASD_FERME 145
+
+#define PINCE_OUVERTE 100
+#define PINCE_FERMEE 3
+
+// ----- Sharp ----- //
+
+#define SHARP_D A4
+#define SHARP_DG A3
+#define SHARP_DD A5
+#define SHARP_AG A2
+#define SHARP_AD A1
+
+#endif

--- a/main.cpp	Tue Jan 05 15:55:56 2016 +0100
+++ b/main.cpp	Thu Jan 07 13:56:38 2016 +0100
@@ -1,4 +1,5 @@
 #include "Odometry.h"
+#include "Map/Map.h"
 
 #define dt 10000
 #define ATTENTE 0
@@ -24,16 +25,27 @@
 /** Debut du programme */
 int main(void)
 {
+    double angle_v = 2*PI/5;
+    double distance_v = 200.0;
+    std::vector<SimplePoint> path;
+    Map map;
+    
+    map.build();
     init();
-    //roboclaw.SpeedAccelDeccelPositionM1M2(ADR, accel_angle, vitesse_angle, deccel_angle, -116878, accel_angle, vitesse_angle, deccel_angle, 116878, 1);
-    odo.GotoXY(1800,1500);
-    odo.GotoXY(2500,500);
-    odo.GotoXY(2000,300);
-    odo.GotoXYT(300,1000,0);
-    //for(int n=0; n<40; n++) odo.setTheta();
-    odo.GotoThet(-PI);
-    odo.GotoThet(0);
+
+    map.Astar(0, 1000, 2000, 1000, 1);
+    path = map.path;
+    
+    for(int i=0; i<path.size();i++)
+        odo.GotoXYT(path[i].x, path[i].y, 0);
+        
+    //odo.GotoXYT(500, 50, 0);
+    //odo.GotoXYT(200, 0, 0);
+    //odo.GotoXYT(0, 0, 0);
+
+    //odo.GotoThet(-PI/2);
     wait(2000);
+    //odo.GotoXYT(2250,500,0);
     while(1);
 }
 
@@ -43,7 +55,7 @@
     pc.printf("Hello from main !\n\r");
     wait_ms(500);
     
-    odo.setPos(300, 1000, 0);
+    odo.setPos(0, 0, 0);
     roboclaw.ForwardM1(ADR, 0);
     roboclaw.ForwardM2(ADR, 0);
     
@@ -58,8 +70,6 @@
 {
     odo.update_odo();
     //pc.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", odo.getX(), odo.getY(), odo.getTheta()*180/PI);
-    //pc.printf("Theta : %3.2f\n\r", odo.getTheta()*180/PI);
-    pc.printf("X : %4.2f\n\r", odo.getX());
     //if(pc.readable()) if(pc.getc()=='l') led = !led;
 }