David Charalampous
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);
+
+ }
+
+
+}
+
+