Revision 58:02dc8328975a, committed 2016-05-04

Comitter:

Jagang

Date:

Wed May 04 19:55:03 2016 +0000

Parent:

55:dfeee9e5c0af

Child:

59:94b44c4a8a5a

Commit message:

Adding ControlleurPince

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ControlleurPince/ControlleurPince.cpp	Wed May 04 19:55:03 2016 +0000
@@ -0,0 +1,42 @@
+
+#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)
+{
+    pos_r = 0;
+    pos_z = 0;
+    pos_l = 0;
+}
+
+
+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)
+    {
+        logger.printf("Move by %f \n\r",z-pos_z);
+        ZMot.mm(z-pos_z);
+        
+        pos_z = z;
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ControlleurPince/ControlleurPince.h	Wed May 04 19:55:03 2016 +0000
@@ -0,0 +1,26 @@
+#ifndef CONTROLLEUR_PINCE_H
+#define CONTROLLEUR_PINCE_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);
+    
+    void home(bool r=true, bool z=true, bool l=true);
+    void setPos(float z, float delta, float center);
+    
+    private:
+    
+        Stepper &RMot, &ZMot, &LMot;
+        
+        DigitalIn &EnR, &EnZ, &EnL;
+        
+        float pos_r,pos_z,pos_l;
+};
+
+#endif

--- a/Functions/defines.h	Wed May 04 16:27:51 2016 +0000
+++ b/Functions/defines.h	Wed May 04 19:55:03 2016 +0000
@@ -18,6 +18,7 @@
 #define dt 10000
 #define HAUT 1
 #define BAS 0
+#define DELAY 0.007
 
 extern Serial logger;
 extern RoboClaw roboclaw;

--- a/Functions/func.h	Wed May 04 16:27:51 2016 +0000
+++ b/Functions/func.h	Wed May 04 19:55:03 2016 +0000
@@ -22,3 +22,5 @@
 void goHomeR(void);
 void checkAround(void);
 void update_main(void);
+
+//float ramp(float currentIn, float minIn, float maxIn, float minOut, float maxOut, float speed);

--- a/StepperMotor.lib	Wed May 04 16:27:51 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-https://developer.mbed.org/users/sype/code/StepperMotor/#c7011e72f0c7

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/StepperMotor/Stepper.cpp	Wed May 04 19:55:03 2016 +0000
@@ -0,0 +1,50 @@
+#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;
+}
+
+void Stepper::step(int number, int dir, float speed)
+{
+    if (dir == HAUT) {
+        direction = 0;
+    } else if (dir == BAS) {
+        direction = 1;
+    }
+    
+    //  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);
+    }
+}
+
+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()
+{
+    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	Wed May 04 19:55:03 2016 +0000
@@ -0,0 +1,27 @@
+#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);
+    void step(int number, int dir, float speed);
+    void mm(int number, int dir);
+    void mm(float distance);
+    void enable();
+    void disable();
+    
+    int getEndStop() {return minEndStop.read();}
+    
+    float Step_Per_MM;
+private:
+    DigitalOut en;
+    DigitalOut stepPin;
+    DigitalOut direction;
+    DigitalIn minEndStop;
+};
+
+
+#endif
\ No newline at end of file

--- a/main.cpp	Wed May 04 16:27:51 2016 +0000
+++ b/main.cpp	Wed May 04 19:55:03 2016 +0000
@@ -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);
@@ -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);
+
 Ticker ticker;
 //Serial logger(USBTX, USBRX);
 Serial logger(PA_2, PA_3);
@@ -58,8 +62,19 @@
 /* Debut du programme */
 int main(void)
 {
-    logger.printf("Depart homologation !\n\r");
-    homologation();
+    //logger.printf("Depart homologation !\n\r");
+    //homologation();
+    logger.printf("Homming ...\n\r");
+    controlleurPince.home();
+    wait(1);
+    controlleurPince.setPos(10.f,0.f,0.f);
+    wait(1);
+    controlleurPince.setPos(20.f,0.f,0.f);
+    wait(1);
+    controlleurPince.setPos(70.f,0.f,0.f);
+    wait(1);
+    controlleurPince.setPos(20.f,0.f,0.f);
+    logger.printf("Done ...\n\r");
     
     /*drapeau = 0;
     init();