Revision 74:07cdad6e861a, committed 2016-05-05

Comitter:

IceTeam

Date:

Thu May 05 05:49:41 2016 +0200

Parent:

73:538d6627ae2f

Parent:

71:5590dbe8393a

Child:

75:195dd2bb13a3

Commit message:

Fusion

Changed in this revision

--- a/AX12/AX12.cpp	Thu May 05 05:48:00 2016 +0200
+++ b/AX12/AX12.cpp	Thu May 05 05:49:41 2016 +0200
@@ -98,6 +98,19 @@
     return(rVal);
 }
 
+int AX12::setSpeed(int speed)
+{
+    char data[2];
+    
+    data[0] = speed & 0xFF;
+    data[1] = speed >> 8;
+    
+    // write the packet, return the error code
+    int rVal = write(_ID, 0x20, 2, data);
+
+    return(rVal);
+}
+
 
 // set continuous rotation speed from -1 to 1
 int AX12::setCRSpeed(float speed) {

--- a/AX12/AX12.h	Thu May 05 05:48:00 2016 +0200
+++ b/AX12/AX12.h	Thu May 05 05:49:41 2016 +0200
@@ -134,6 +134,11 @@
      *    1.0 = full speed clock wise
      */
     int setCRSpeed(float speed);
+    
+    /** Set the speed of the servo
+     *
+     */
+    int setSpeed(int speed);
 
 
     /** Set the clockwise limit of the servo

--- a/ControlleurPince/ControlleurPince.cpp	Thu May 05 05:48:00 2016 +0200
+++ b/ControlleurPince/ControlleurPince.cpp	Thu May 05 05:49:41 2016 +0200
@@ -2,12 +2,27 @@
 #include "defines.h"
 #include "ControlleurPince.h"
 
-ControlleurPince::ControlleurPince(Stepper &p_RMot, Stepper &p_ZMot, Stepper &p_LMot, DigitalIn &p_EnR, DigitalIn &p_EnZ, DigitalIn &p_EnL):
-    RMot(p_RMot), ZMot(p_ZMot), LMot(p_LMot), EnR(p_EnR), EnZ(p_EnZ), EnL(p_EnL)
+ControlleurPince::ControlleurPince(Stepper &p_RMot, Stepper &p_ZMot, Stepper &p_LMot, DigitalIn &p_EnR, DigitalIn &p_EnZ, DigitalIn &p_EnL, AX12 &_Lax12, AX12 &_Rax12) :
+    RMot(p_RMot), ZMot(p_ZMot), LMot(p_LMot), EnR(p_EnR), EnZ(p_EnZ), EnL(p_EnL), Lax12(_Lax12), Rax12(_Rax12)
 {
     pos_r = 0;
     pos_z = 0;
     pos_l = 0;
+    
+    
+}
+
+void ControlleurPince::init()
+{
+    Lax12.setMode(0);
+    wait(0.01);
+    Rax12.setMode(0);
+    wait(0.01);
+    
+    Lax12.setMaxTorque(200);
+    wait(0.01);
+    Rax12.setMaxTorque(200);
+    wait(0.01);
 }
 
 
@@ -34,9 +49,53 @@
 {
     if(z > 0.f && z < 100.f)
     {
-        logger.printf("Move by %f \n\r",z-pos_z);
-        ZMot.mm(z-pos_z);
-        
+        ZMot.mm(z-pos_z,true);
         pos_z = z;
     }
+    
+    if(delta >= 0 || center >= -500)
+    {
+        if(delta < 0)
+            delta = 130 - pos_r - pos_l;
+        if(center < -500)
+            center = - pos_r/2 + pos_l/2;
+            
+        
+        
+        /*
+        => pos_l = center*2 + pos_r
+        delta = 130 - pos_r - center*2 - pos_r
+        => 2*pos_r = 130 - center*2 - delta
+        => pos_r = (130-delta)/2 - center
+        => pos_l = (130-delta)/2 + center
+        */
+        
+        float r = (130.f-delta)/2.f - center;
+        float l = (130.f-delta)/2.f + center;
+        
+        RMot.mm(r-pos_r,true);
+        pos_r = r;
+        
+        LMot.mm(l-pos_l,true);
+        pos_l = l;
+        
+    }
+    
+    while(! (RMot.done() && ZMot.done() && LMot.done()))
+        wait(DELAY);
+}
+
+void ControlleurPince::close()
+{
+    Lax12.setGoal(150);
+    Rax12.setGoal(150);
+    wait(0.2);
+}
+
+
+void ControlleurPince::open()
+{
+    Lax12.setGoal(100);
+    Rax12.setGoal(200);
+    wait(0.2);
 }
\ No newline at end of file

--- a/ControlleurPince/ControlleurPince.h	Thu May 05 05:48:00 2016 +0200
+++ b/ControlleurPince/ControlleurPince.h	Thu May 05 05:49:41 2016 +0200
@@ -1,6 +1,7 @@
 #ifndef CONTROLLEUR_PINCE_H
 #define CONTROLLEUR_PINCE_H
 
+//#include "defines.h"
 #include "Stepper.h"
 #include "mbed.h"
 
@@ -9,10 +10,14 @@
     
     public:
     
-    ControlleurPince(Stepper &p_RMot, Stepper &p_ZMot, Stepper &p_LMot, DigitalIn &p_EnR, DigitalIn &p_EnZ, DigitalIn &p_EnL);
-    
-    void home(bool r=true, bool z=true, bool l=true);
-    void setPos(float z, float delta, float center);
+        ControlleurPince(Stepper &p_RMot, Stepper &p_ZMot, Stepper &p_LMot, DigitalIn &p_EnR, DigitalIn &p_EnZ, DigitalIn &p_EnL, AX12 &_Lax12, AX12 &_Rax12);
+        
+        void init();
+        void home(bool r=true, bool z=true, bool l=true);
+        void setPos(float z, float delta, float center = -600);
+        
+        void close();
+        void open();
     
     private:
     
@@ -20,6 +25,8 @@
         
         DigitalIn &EnR, &EnZ, &EnL;
         
+        AX12 &Lax12, &Rax12;
+        
         float pos_r,pos_z,pos_l;
 };
 

--- a/StepperMotor/Stepper.cpp	Thu May 05 05:48:00 2016 +0200
+++ b/StepperMotor/Stepper.cpp	Thu May 05 05:49:41 2016 +0200
@@ -10,15 +10,33 @@
     Step_Per_MM = step_per_mm;
 }
 
-void Stepper::step(int number, int dir, float speed)
+bool Stepper::step(int _number, int _dir, float _speed, bool _async)
 {
-    if (dir == HAUT) {
-        direction = 0;
-    } else if (dir == BAS) {
-        direction = 1;
+    if(done())
+    {
+        dir = _dir;
+        number = _number;
+        speed = _speed;
+        async = _async;
+        
+        if (dir == HAUT)
+            direction = 0;
+        else if (dir == BAS)
+            direction = 1;
+        
+        ticker.attach(this,&Stepper::tick,_speed);
+        if(!_async)
+            while(!done())
+                wait(speed);
+        
+        return true;
     }
     
-    //  Step...
+    return false;
+    
+    
+    
+    /*  Step...
     for(int i=0; i<number && !(minEndStop.read() == 0 && dir == BAS); i++)
     {
         stepPin = 1;
@@ -26,18 +44,39 @@
         stepPin = 0;
         wait_us(5);
         wait(speed);
+    }*/
+}
+
+bool Stepper::mm(int _number, int _dir, bool _async)
+{
+    return step(_number*Step_Per_MM, _dir, DELAY-0.001, _async);
+}
+
+bool Stepper::mm(float _distance, bool _async)
+{
+    return step(abs(_distance)*Step_Per_MM, (_distance>0?HAUT:BAS), DELAY-0.001, _async);
+}
+
+bool Stepper::done()
+{
+    return number == 0;
+}
+
+void Stepper::tick()
+{
+    stepPin = 1;
+    wait_us(5);
+    stepPin = 0;
+    
+    number--;
+    
+    if(number <= 0 || (minEndStop.read() == 0 && dir == BAS))
+    {
+        ticker.detach();
+        number = 0;
     }
 }
 
-void Stepper::mm(int number, int dir)
-{
-    step(number*Step_Per_MM, dir, DELAY-0.001);
-}
-
-void Stepper::mm(float distance)
-{
-    step(abs(distance*Step_Per_MM), (distance>0?HAUT:BAS), DELAY-0.001);
-}
 
 void Stepper::enable()
 {

--- a/StepperMotor/Stepper.h	Thu May 05 05:48:00 2016 +0200
+++ b/StepperMotor/Stepper.h	Thu May 05 05:49:41 2016 +0200
@@ -7,20 +7,32 @@
 {
 public:
     Stepper(PinName _en, PinName _stepPin, PinName dir, PinName _minEndStop, float step_per_mm);
-    void step(int number, int dir, float speed);
-    void mm(int number, int dir);
-    void mm(float distance);
+    bool step(int _number, int _dir, float _speed, bool _async=false);
+    bool mm(int _number, int _dir, bool _async=false);
+    bool mm(float _distance, bool _async=false);
+    
+    bool done();
+    
     void enable();
     void disable();
     
     int getEndStop() {return minEndStop.read();}
     
+    void tick();
+    
     float Step_Per_MM;
 private:
     DigitalOut en;
     DigitalOut stepPin;
     DigitalOut direction;
     DigitalIn minEndStop;
+    
+    int number;
+    int dir;
+    float speed;
+    bool async;
+    Ticker ticker;
+    
 };
 
 

--- a/main.cpp.orig	Thu May 05 05:48:00 2016 +0200
+++ b/main.cpp.orig	Thu May 05 05:49:41 2016 +0200
@@ -1,6 +1,8 @@
 #include "func.h"
 #include "map.h"
 
+#include "ControlleurPince.h"
+
 /* Déclaration des différents éléments de l'IHM */
 
 DigitalIn CAMP(PA_15);
@@ -22,8 +24,8 @@
 DigitalOut blanc(PC_6);
 DigitalOut rouge(PC_8);
 
-//AX12 left_hand(PA_9, PA_10, 4, 250000);
-//AX12 right_hand(PA_9, PA_10, 1, 250000);
+AX12 left_hand(PA_9, PA_10, 3, 250000);
+AX12 right_hand(PA_9, PA_10, 2, 250000);
 
 /* Sharp */
 AnalogIn capt1(PA_4);
@@ -32,9 +34,9 @@
 AnalogIn capt4(PC_0);
 
 /* Moteurs pas à pas */
-Stepper RMot(NC, PA_8, PC_7, 4);
-Stepper ZMot(NC, PB_4, PB_10, 5);
-Stepper LMot(NC, PB_5, PB_3, 4);
+Stepper RMot(NC, PA_8, PC_7, PB_15, 4);
+Stepper ZMot(NC, PB_4, PB_10, PB_14, 5);
+Stepper LMot(NC, PB_5, PB_3, PB_13, 4);
 /* Fins de course */
 InterruptIn EndR(PB_15);
 InterruptIn EndZ(PB_14);
@@ -43,6 +45,8 @@
 DigitalIn EnZ(PB_14);
 DigitalIn EnL(PB_13);
 
+ControlleurPince controlleurPince(RMot, ZMot, LMot, EnR, EnZ, EnL, left_hand, right_hand);
+
 Ticker ticker;
 //Serial logger(USBTX, USBRX);
 Serial logger(PA_2, PA_3);
@@ -58,8 +62,53 @@
 /* Debut du programme */
 int main(void)
 {
-    logger.printf("Depart homologation !\n\r");
-    homologation();
+<<<<<<< local
+    init();
+    map m(&odo, NULL, &controlleurPince, VERT, 1);
+    m.Execute(0);
+    m.Execute();
+=======
+    roboclaw.ForwardM1(0);
+    roboclaw.ForwardM2(0);
+    
+    //logger.printf("Depart homologation !\n\r");
+    //homologation();
+    
+    controlleurPince.init();
+    controlleurPince.home();
+    wait(1);
+    controlleurPince.open();
+    logger.printf("z 10 \r\n");
+    controlleurPince.setPos(10,-1);
+    wait(1);
+    logger.printf("z 20 \r\n");
+    controlleurPince.setPos(20,-1);
+    wait(1);
+    controlleurPince.close();
+    logger.printf("d 30 \r\n");
+    controlleurPince.setPos(-1,30);
+    wait(1);
+    logger.printf("c 50 \r\n");
+    controlleurPince.setPos(-1,-1,50);
+    wait(1);
+    logger.printf("d 0 \r\n");
+    controlleurPince.setPos(-1,0);
+    
+    while(1);
+    
+    
+    /*wait(1);
+    logger.printf("set pos 20 ...\n\r");
+    controlleurPince.setPos(20.f,0.f,0.f);
+    wait(1);
+    logger.printf("set pos 70 ...\n\r");
+    controlleurPince.setPos(70.f,0.f,0.f);
+    wait(1);
+    logger.printf("set pos 20 ...\n\r");
+    controlleurPince.setPos(20.f,0.f,0.f);
+    logger.printf("Done ...\n\r");*/
+    
+>>>>>>> other
     /*drapeau = 0;
     init();
 
@@ -68,6 +117,11 @@
     m.addObs(obsCarr (1500, 750, 220, 220));
     m.addObs(obsCarr (1500, 1250, 220, 220));
 
+    init();    
+    
+    int cote = MAP_RIGHTSIDE;
+    /*map m(&odo);
+    m.Build(cote);
     m.Execute(1000,1000);
     m.Execute(1500,1000);
     m.Execute(1500,1500);
@@ -86,23 +140,15 @@
     logger.baud(9600);
     logger.printf("Hello from main !\n\r");
 
-    init_interrupt();
-    goHome();
-
-    SCouleur = VERT;
-    LEDV = 1;
-    LEDR = 0;
-
-    odo.setPos(110, 1000, 0);
-    roboclaw.ResetEnc();
-    roboclaw.ForwardM1(0);
-    roboclaw.ForwardM2(0);
-    wait_ms(500);
-    while(1);
+    controlleurPince.home();
+    
     //depart();
     init_interrupt();
-    wait_ms(100);
-    while(START==0);
+    wait_ms(1);
+    while (CAMP == 0);
+    while (CAMP == 1);
+    
+    //while(START==0);
     logger.printf("End init\n\r");
 }