Revision 92:f09f55aa992b, committed 2016-05-06

Comitter:

IceTeam

Date:

Fri May 06 15:25:21 2016 +0000

Parent:

91:65fb6b9f3932

Child:

93:c0b040954eac

Commit message:

Rajout de ControlleurPince & StepperMotor;

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ControlleurPince/ControlleurPince.cpp	Fri May 06 15:25:21 2016 +0000
@@ -0,0 +1,101 @@
+
+#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, 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);
+}
+
+
+void ControlleurPince::home(bool r, bool z, bool l)
+{
+    if(r)
+    {
+        while(EnR==true) RMot.step(1, BAS, DELAY);
+        pos_r = 0;
+    }
+    if(z)
+    {
+        while(EnZ==true) ZMot.step(1, BAS, DELAY);
+        pos_z = 0;
+    }
+    if(l)
+    {
+        while(EnL==true) LMot.step(1, BAS, DELAY);
+        pos_l = 0;
+    }
+}
+
+void ControlleurPince::setPos(float z, float delta, float center)
+{
+    if(z > 0.f && z < 100.f)
+    {
+        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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ControlleurPince/ControlleurPince.h	Fri May 06 15:25:21 2016 +0000
@@ -0,0 +1,33 @@
+#ifndef CONTROLLEUR_PINCE_H
+#define CONTROLLEUR_PINCE_H
+
+//#include "defines.h"
+#include "Stepper.h"
+#include "mbed.h"
+
+class ControlleurPince
+{
+    
+    public:
+    
+        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:
+    
+        Stepper &RMot, &ZMot, &LMot;
+        
+        DigitalIn &EnR, &EnZ, &EnL;
+        
+        AX12 &Lax12, &Rax12;
+        
+        float pos_r,pos_z,pos_l;
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/StepperMotor/Stepper.cpp	Fri May 06 15:25:21 2016 +0000
@@ -0,0 +1,89 @@
+#include "defines.h"
+#include "Stepper.h"
+#include "mbed.h"
+
+Stepper::Stepper(PinName _en, PinName _stepPin, PinName _dir, PinName _minEndStop, float step_per_mm):en(_en),
+    stepPin(_stepPin),
+    direction(_dir),
+    minEndStop(_minEndStop)
+{
+    Step_Per_MM = step_per_mm;
+}
+
+bool Stepper::step(int _number, int _dir, float _speed, bool _async)
+{
+    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;
+    }
+    
+    return false;
+    
+    
+    
+    /*  Step...
+    for(int i=0; i<number && !(minEndStop.read() == 0 && dir == BAS); i++)
+    {
+        stepPin = 1;
+        wait_us(5);
+        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::enable()
+{
+    en = 0;
+}
+
+void Stepper::disable()
+{
+    en = 1;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/StepperMotor/Stepper.h	Fri May 06 15:25:21 2016 +0000
@@ -0,0 +1,39 @@
+#ifndef MBED_STEPPER_H
+#define MBED_STEPPER_H
+
+#include "mbed.h"
+
+class Stepper
+{
+public:
+    Stepper(PinName _en, PinName _stepPin, PinName dir, PinName _minEndStop, float step_per_mm);
+    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;
+    
+};
+
+
+#endif
\ No newline at end of file

--- a/entete.h	Fri May 06 13:36:06 2016 +0000
+++ b/entete.h	Fri May 06 15:25:21 2016 +0000
@@ -4,6 +4,11 @@
 #include "RoboClaw/RoboClaw.h"
 #include "mbed.h"
 
+/* Classes du projet : 
+AX12
+Roboclaw
+*/
+
 extern BusOut drapeau;
 extern RoboClaw roboclaw;
 extern AnalogIn Rcapt1;
@@ -26,6 +31,7 @@
 // Fonctions main.cpp
 void Sharps();
 void endFonc ();
+void init();
 
 //vFonctions deplacement.cpp
 void GotoThet (float timer, int signe);

--- a/main.cpp	Fri May 06 13:36:06 2016 +0000
+++ b/main.cpp	Fri May 06 15:25:21 2016 +0000
@@ -1,4 +1,5 @@
 #include "entete.h"
+
 #include "AX12/AX12.h"
 
 DigitalIn CAMP(PA_15);
@@ -24,34 +25,7 @@
 /* Debut du programme */
 int main(void)
 {
-    roboclaw.ForwardM1(0);
-    roboclaw.ForwardM2(0);
-    Ticker ticker;
-    Timeout end;
-    ticker.attach(&Sharps, 0.01);
 
-    umbrella.setMode(0);
-    umbrella.setMaxTorque(200);
-    umbrella.setGoal(150);
-    wait(1);
-    umbrella.setGoal(160);
-    wait(1);
-    umbrella.setGoal(150);
-    wait(1);
-    umbrella.setMaxTorque(0);
-    
-    while(START == 1)
-    {
-        LEDR = 1;
-        LEDV = 1;
-        wait(0.5);
-        LEDR = 0;
-        LEDV = 0;
-        wait(0.5);
-    }
-   
-    wait(1);
-    depart();
     
     if (SCouleur == VERT) {
         end.attach(&endFonc, 90);
@@ -70,7 +44,7 @@
         GotoDist(3.5);
         GotoThet(-3.04);
         R_SEUIL_SHARP = 1; 
-        GotoDist(4.1);
+        GotoDist(4.5);
     }
     else if (SCouleur == VIOLET) {  
         end.attach(&endFonc, 90);
@@ -89,7 +63,7 @@
         GotoDist(3.5);
         GotoThet(3.04);
         R_SEUIL_SHARP = 1; 
-        GotoDist(4.1);
+        GotoDist(4.5);
     }
     else if (SCouleur == NOIR) {
         TestDist3(2,2);
@@ -135,3 +109,34 @@
     umbrella.setGoal(300);
     while(1);
 }
+
+void init() {
+    roboclaw.ForwardM1(0);
+    roboclaw.ForwardM2(0);
+    Ticker ticker;
+    Timeout end;
+    ticker.attach(&Sharps, 0.01);
+
+    umbrella.setMode(0);
+    umbrella.setMaxTorque(200);
+    umbrella.setGoal(150);
+    wait(1);
+    umbrella.setGoal(160);
+    wait(1);
+    umbrella.setGoal(150);
+    wait(1);
+    umbrella.setMaxTorque(0);
+    
+    while(START == 1)
+    {
+        LEDR = 1;
+        LEDV = 1;
+        wait(0.5);
+        LEDR = 0;
+        LEDV = 0;
+        wait(0.5);
+    }
+   
+    wait(1);
+    depart();
+}