![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Program wich pilot a step motor board.
Diff: Dplct_Lin_1_3_2.cpp
- Revision:
- 0:b031f05749d0
--- /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); + + } + + +} + +