Revision 0:b031f05749d0, committed 2014-06-30

Comitter:

dcharala

Date:

Mon Jun 30 11:54:00 2014 +0000

Commit message:

This programm permit to drive a specific card forstep motors

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Dplct_Lin_1_3_2.cpp	Mon Jun 30 11:54:00 2014 +0000
@@ -0,0 +1,477 @@
+#include "mbed.h"
+
+#include <sstream>
+#include <string>
+// Version 1.0 : Fait uniquement marcher les moteurs d'une position à une autre donnée en nb d'impulsion
+// Version 1.1.0 : non utilisation du PWM mais du mode ticker pour piloter le moteur
+//validation 1.1 : Rajout d'une rampe sur le pin de control du moteur en 500ms, et gestion du pin par ticker au lieu d'une PWM
+// fonction stop?
+
+// Version 1.2.0 : integration du code QEI(quadrature encoder input) , nécessiste 2 ports : pin 15 et 16
+// position transformé directement en cm
+
+// Version 1.3 : mesure de temps entre impulsion  + résolution encodeur 1/800 e de tour au lieu de 1/400e
+
+// version 1.3.2 : envoi de la position réelle 
+
+//A little test in order to slipt a string in two part, as following :
+// "This string" => arg[0]="This"  arg[1]="string"
+// After a test with split
+
+
+
+//This programm is designed in order to move on a z-axis a device by a step motot
+// a card sold by selectronic controlling a step motor through a L297 and a L6203 from ST
+//see:
+//http://www.selectronic.fr/c/robotique-moteurs-accessoires/modules-de-commande/commandes-de-moteur-pas-a-pas/carte-de-commande-pour-moteur-pas-a-pas-a-l6203-version-montee.html
+//Ref. 6950-11
+
+//Config LCD
+//TextLCD lcd(p14,p19,p24,p23,p22,p21); //rs, e, d4-d7
+
+
+//config LEDS
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+
+//In some Future version :
+
+DigitalOut PinEnable(p17);
+DigitalOut PinControl(p18);
+
+DigitalOut Pin_HF_Pas(p27);
+
+// QEI Voie A et B
+InterruptIn VoieA(p16);
+InterruptIn VoieB(p15);
+Timer t;
+//PwmOut Clock(p25); // CHANGE :: tranforn in Digital OUt and put a ticker on
+DigitalOut Clock(p25);
+Ticker Clocker;
+Timeout flipper;
+DigitalOut PinDrct(p28);
+DigitalOut PinReset(p26);
+//InterruptIn PinCount(p29); //counter of PWM so it's count the number of step of the motor. pin free from 1.1 version
+
+/*union FloatBytes {
+    char c[4];
+    float f;
+
+
+};*/
+
+float speed=5e-3; // speed is the period of the motot pulse by default the frequency is 200Hz => 1.2 cm/s
+
+// Variables of position : given here vers 1.0 like an int af after 1.1 in cm as an float.
+float NewPos=0.0;
+//int position=0;
+float dir=0.0;
+int RealPosition=0;
+float p=1.0/200.0*1.2;
+int run=0;
+int Stop=0;
+float positionCm=0;// positionCm=position/200*1.2
+float RealPositionCm=0;// RealPositionCm=RealPosition/400*1.2
+int i=0;
+int count=0;
+
+Serial pc(USBTX,USBRX); //tx, rx
+
+//QEI variables :
+
+float periode=0;
+int k=0;
+float drpm=1.0/800.0*1.2;
+float velocity=0; //in mm/s
+
+
+float array_time_pos[40];
+
+float array_time_pulse[14];
+
+//*************************
+
+char s1[20],s2[20];// s1 and s2 will be the two arg off the string
+// I will do just an echo as following:
+// string1 = ...
+//string 2 = ...
+
+//TO DO LIST 1.1
+/*
+func POW    =>  DONE
+func PAS    =>  DONE
+func VIT    =>  DONE
+func DRCT   =>  DONE
+func ZERO   =>  DONE
+func POS    =>  DONE
+func MOVE   =>  DONE
+
+
+*/
+//TO DO LIST 1.2
+/*Integration QEI :
+Integration QEI :
+- injection code init dans main : FAIT
+- inclure fonction fall et rise : FAIT
+- inclure déclaration de variables utilisées dans fall and rise :FAIT
+- exploitation de l'information QEI dans le code : à faire
+- non utilisation de la variable timeout t : pour l'instant à voir
+
+*/
+
+//values return by text the status of the controller
+
+
+void Values()
+{
+    int pas=Pin_HF_Pas;
+    int power=PinEnable;
+
+
+    pc.printf("\n\rStates\n\r");
+    pc.printf("Half/Full %d\n",pas); //half 0 full 1
+    pc.printf("Period_speed %f\n",speed);
+    pc.printf("Actual_position %fcm\n",positionCm);
+    pc.printf("Real_position %fcm\n",RealPositionCm);
+    pc.printf("Power %d\n@",power);
+
+}
+// Moving() permit to manage the movement of the step motor
+void compt_rise_A(void)
+{
+
+    //t.start();
+    if(VoieB)     {
+
+        RealPositionCm=RealPositionCm+drpm;
+    } else {
+
+        RealPositionCm=RealPositionCm-drpm;
+
+    }
+    
+    array_time_pos[i]=t.read();
+   //i++;
+}
+
+void compt_rise_B(void)
+{
+    //t.start();
+    if(VoieA)     {
+
+        RealPositionCm=RealPositionCm-drpm;
+    } else {
+
+        RealPositionCm=RealPositionCm+drpm;
+
+    }
+
+    array_time_pos[i]=t.read();
+   //i++;
+}
+
+void compt_fall_A(void)
+{
+    // t.stop();
+    //periode=t.read()*2;
+    // velocity=Pas/(periode*PPR*2);//in µm/s
+
+    //t.reset();
+    if(!VoieB) {
+
+        RealPositionCm=RealPositionCm+drpm;
+    } else {
+
+        RealPositionCm=RealPositionCm-drpm;
+    }
+    
+    array_time_pos[i]=t.read();
+   //i++;
+}
+
+void compt_fall_B(void)
+{
+    // t.stop();
+    //periode=t.read()*2;
+    // velocity=Pas/(periode*PPR*2);//in µm/s
+
+    //t.reset();
+    if(!VoieA) {
+
+        RealPositionCm=RealPositionCm-drpm;
+    } else {
+
+        RealPositionCm=RealPositionCm+drpm;
+    }
+    
+    array_time_pos[i]=t.read();
+   //i++;
+}
+LocalFileSystem local("local");               // Create the local filesystem under the name "local"
+FILE *fp = fopen("/local/out.txt", "w");  // Open "out.txt" on the local file system for writing
+
+
+/*void Write()
+{
+     int j=0;
+    fp = fopen("/local/out.txt", "w");
+    for( j =0;  j<=i ; j++ ) {
+        fprintf(fp, "%f\n",array_time_pos[j]);
+    }
+
+    fclose(fp);
+    j=0;
+
+    fp=fopen("/local/out_pulse.txt", "w");
+    for( j =0;  j<=k ; j++ ) {
+        fprintf(fp, "%f\n",array_time_pulse[j]);
+    }
+    fclose(fp);
+    
+
+
+}*/
+
+void flip(){
+    
+    Clock=1;
+    
+    }
+
+void Moving()
+{
+    if(positionCm*dir>=NewPos*dir||Stop==1) {
+
+        Clock=1;// set the PWM at 1 so it switch off the controller at pull up
+        Clocker.detach();// this action detach the funxctio Moving() from PinCount
+        PinEnable=0;
+        pc.printf("\narrived at %fcm\nReal_position %fcm\n@",positionCm,RealPositionCm);
+        //Write();
+        Stop=0;
+        t.stop();
+        t.reset();
+        k=0;
+        i=0;
+    } else {
+        Clock=0;
+        array_time_pulse[k]= t.read();
+       //k++;
+        
+        flipper.attach_us(&flip,500);        
+        if(PinDrct==1) {
+            //position++;
+            positionCm=positionCm+p;
+        }
+        if(PinDrct==0) {
+            //position--;
+            positionCm=positionCm-p;
+        }
+    }
+
+}
+
+
+
+
+void POW(char p)//DONE
+// rend "enable" l'action des moteur
+{
+    //To be optimized later
+    if(p=='1') {
+        PinEnable=1;
+    }
+    if(p=='0') {
+        PinEnable=0;
+    }
+}
+
+
+void PAS(char t)//DONE
+{
+    if(t=='0') {// FULL
+        Pin_HF_Pas=0;
+        p=1.0/200.0*1.2;
+    }
+    if(t=='1') {
+        Pin_HF_Pas=1;// Half
+        p=1.0/400.0*1.2;
+    }
+
+}
+
+
+void VIT(char SpeedTxt[7])//DONE
+{
+    speed=atof(SpeedTxt); // speed is a period of frenquency of the step motor
+}
+
+
+void DRCT(char t)//DONE
+{
+    if(t=='0')
+        PinDrct=0;
+    if(t=='1')
+        PinDrct=1;
+}
+
+
+void ZERO()//DONE
+{
+    positionCm=0.0;
+
+}
+
+
+void POS(char PosTxt[7])//DONE
+{
+    positionCm = atof(PosTxt);
+    RealPosition= positionCm;
+
+}
+
+void MOVE(char PosTxt[7])//DOING
+{
+    float f,diff;
+    int dirdir=0;
+//   compt=0;
+    k=0;
+    i=0;
+
+
+    f=1/speed;
+    NewPos = atof(PosTxt);
+    diff=NewPos-positionCm;
+    diff=abs(diff);
+    if(diff>0) {
+        POW('1');
+        if(NewPos>positionCm) {
+            PinDrct=1;
+            //position=position+2; // code non nécessaire depuis vers 1.1
+        } else {
+            //position=position-2; // code non nécessaire depuis vers 1.1
+            PinDrct=0;
+        }
+        dirdir=PinDrct;
+        dir=dirdir;
+        dir=dir*2.0-1.0;
+        pc.printf("\n\rPos= %f Newpos= %f \nDirection = %f\n Frequency = %f\n\r@",positionCm,NewPos,dir,f);
+        //distance of run unit ste (version 1.0) and in version 1.1 : the unit will be the cm
+        //PinCount.rise(&Moving);
+        //Clock=0.5; // switch on clock count
+        t.reset();
+        t.start();
+        Clocker.attach(&Moving,speed);
+    }
+
+}
+
+
+
+void reception()
+{
+    //char s[]="";
+    sprintf(s1,"");
+    sprintf(s2,"");
+    //long longueur=100000;
+
+    if(pc.readable()) {
+        pc.scanf("%s %s",&s1,&s2);
+
+        if (strcmp(s1,"POW")==0) {
+            POW(s2[0]);
+            pc.printf("POW\n\r@");
+        }
+        if (strcmp(s1,"PAS")==0) {
+            PAS(s2[0]);
+            pc.printf("\n\r@");
+        }
+        if (strcmp(s1,"VIT")==0) {
+            VIT(s2);
+            pc.printf("\n\r@");
+        }
+        if (strcmp(s1,"DRCT")==0) {
+            DRCT(s2[0]);
+            pc.printf("\n\r@");
+        }
+        if (strcmp(s1,"ZERO")==0) {
+            ZERO();
+            pc.printf("\n\r@");
+
+        }
+        if (strcmp(s1,"STOP")==0) {
+            Stop=1;
+
+            pc.printf("\n Stop received\r@");
+        }
+
+
+        if (strcmp(s1,"MOVE")==0) {
+            MOVE(s2);
+            pc.printf("\nMoving\n\r@");
+
+        }
+        if (strcmp(s1,"POS")==0) {
+            //function to see array to integer or something like that
+            // in order to transform a string (s2) into an int and after a float(version 1.1)
+            float oldpos=positionCm;
+            positionCm=atof(s2);
+            RealPositionCm=positionCm;
+            count++;
+
+            pc.printf("Old position : %f\n\rNew Position : %f\n@",oldpos,positionCm);
+        }
+
+        if (strcmp(s1,"CONT")==0) {
+            PinControl=atoi(s2);
+        }
+
+        if (strcmp(s1,"POS?")==0) {
+            pc.printf("\nPosition = %f cm\nRealPosition = %f cm\n\@",positionCm,RealPositionCm);
+        }
+        if (strcmp(s1,"VAL?")==0) {
+            Values();
+        }
+
+
+    }
+}
+
+
+int main()
+{
+    PinControl=0;
+    PinReset=1; //reset on 0
+    PinDrct=1;
+    Pin_HF_Pas=0; //full step:0   Half step:1
+    // could be programmable later if necessairly.
+    //pc.baud(115200);
+    pc.baud(921600);
+    
+    VoieA.mode(PullUp);
+    VoieA.rise(&compt_rise_A);
+    VoieA.fall(&compt_fall_A);
+    VoieB.mode(PullUp);
+    VoieB.rise(&compt_rise_B);
+    VoieB.fall(&compt_fall_B);
+    pc.printf("MBED\r\n\r@");
+    fclose(fp);
+
+
+
+
+
+    //PinCount.mode(PullDown);
+    positionCm=0;
+    Clock=1;
+    while(1) {
+
+        pc.attach(&reception);
+
+    }
+
+
+}
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TextLCD.lib	Mon Jun 30 11:54:00 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/simon/code/TextLCD/#44f34c09bd37

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Mon Jun 30 11:54:00 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/5e5da4a5990b
\ No newline at end of file