Sist. Piccolo
Esta última actualización contiene el código funcional en las 10 funciones.
Dependencies: mbed
Fork of
jc
by 
Sist. Piccolo
Diff: main.cpp
- Revision:
- 0:9aa960516df7
- Child:
- 1:a9b3fdab473e
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Mon Nov 20 04:28:42 2017 +0000
@@ -0,0 +1,477 @@
+#include "mbed.h"
+#include "math.h"
+#include "cstdlib"
+#define MAXPOS 255
+#define SS_TIME 500
+#define POSDRAW 300
+#define BUFF_SIZE 6
+#define COMM_N 0
+#define INITPARAMETER 1
+#define DEBUG 1
+#define BUFF_POS 6
+
+PwmOut myServoX(PB_3); //MOTOR EJE X
+PwmOut myServoY(PB_4); //MOTOR EJE Y
+PwmOut myServoZ(PB_10);//MOTOR EJE Z
+
+Serial command(USBTX, USBRX); //COMUNICACION SERIAL
+Serial Sdebug(USBTX, USBRX);
+DigitalOut led(LED1); //DECLARACION LED DE LA PLACA
+InterruptIn mybutton(USER_BUTTON);
+
+// COMANDOS
+#define LED 0
+#define PUNTO 1
+#define LINE 2
+#define RECTANGLE 3
+#define CIRCLE 4
+#define STOP 5
+#define PAUSE 6
+#define REANUDAR 7
+#define RESOLUCION 8
+#define HOME 9
+#define TIEMPO_DE_PASO 10
+
+Ticker ticker1;
+Timeout timeout;
+
+int rap;
+int RSTEP = 5;
+double abajo=7;
+double arriba=0;
+double k,l,r,tp=50,paso=0;
+int p=0;
+
+//*****************************************************************************
+// COMANDO MOVER MOTOR
+// |POS 1|POS 2|POS 3|POS 4| POS 5| POS 6| POS 7|
+// | < | #C | a | b | c | d | > |
+//
+// #C -> indica el comando
+// a,b,c,d parametros del comando
+// <,> -> inicio, y fin de comando
+// el inicio de comando no se almacena en el buffer
+//*****************************************************************************
+// VARIABLES PARA DEFINIR EL COMMANDO
+
+uint8_t buffer_command[BUFF_SIZE]={0,0,0,0,0,0};
+float buffer_POS [BUFF_POS]= {0,0,0,0,0,0};
+
+void savep();
+void interrupcion();
+
+int coord2us(float coord)
+ {
+ if(0 <= coord <= MAXPOS)
+ return int(750+coord*1900/50);// u6
+ return 750;
+ }
+ void print_num(uint8_t val)
+ {
+ if (val <10)
+ command.putc(val+0x30);
+ else
+ command.putc(val-9+0x40);
+ }
+void print_bin2hex (uint8_t val)
+ {
+ command.printf(" 0x");
+ print_num(val>>4);
+ print_num(val&0x0f);
+ }
+void Read_command()
+ {
+ for (uint8_t i=0; i<BUFF_SIZE;i++)
+ {
+ buffer_command[i]=command.getc();
+
+ }
+ }
+
+void echo_command()
+ {
+ for (uint8_t i=0; i<BUFF_SIZE;i++)
+ {
+ print_bin2hex(buffer_command[i]);
+ }
+ }
+uint8_t check_command()
+ {
+ if (buffer_command[BUFF_SIZE-1]== '>')
+ return 1;
+ return 0;
+ }
+void command_led(uint8_t tm)
+ {
+ #if DEBUG
+ command.printf("%i, segundos", tm);
+ #endif
+ led=1;
+ wait(tm);
+ led=0;
+ }
+void draw()
+ {
+ myServoZ.pulsewidth_us(POSDRAW);
+ wait(1);
+ }
+void nodraw()
+ {
+ myServoZ.pulsewidth_us(MAXPOS);
+ wait(1);
+ }
+ uint8_t SSTIME = 100;
+void tiempo_de_paso(int SST)
+ {
+ SSTIME=SST;
+ #if DEBUG
+ command.printf("\nTiempo en pasos definida en:%i\n",SSTIME);
+ #endif
+ }
+void vertex2d(float x, float y, float z)
+ {
+ int pulseX = coord2us(x);
+ int pulseY = coord2us(y);
+ int pulseZ = coord2us(z);
+ myServoX.pulsewidth_us(pulseX);
+ myServoY.pulsewidth_us(pulseY);
+ myServoZ.pulsewidth_us(pulseZ);
+ wait_ms(SSTIME);
+ }
+
+uint8_t posx_old=0; // posición anterior del eje X
+uint8_t posy_old=0; // posición anterior del eje Y
+uint8_t posz_old=0; // posición anterior del eje Y
+uint8_t ss_time=100; // tiempo de espera para moverse 1 mm en microsegundos
+
+void put_sstime(uint8_t vtime){
+ ss_time=vtime;
+ }
+void sstime(uint8_t x, uint8_t y, uint8_t z)
+{
+ double dx=abs(x-posx_old);
+ double dy=abs(y-posy_old);
+ double dz=abs(z-posz_old);
+ double dist= sqrt(dx*dx+dy*dy+dz*dz);
+ wait_ms((int)(ss_time*dist));
+ posx_old=x;
+ posy_old=y;
+ posy_old=z;
+ }
+void initdraw(float x, float y)
+ {
+ #if DEBUG
+ Sdebug.printf("inicia dibujo \n");
+ #endif
+ vertex2d (x,y,abajo);
+ wait(1);
+ draw();
+ }
+
+void resolucion(int res){
+ RSTEP = res;
+ #if DEBUG
+ command.printf("Resolucion definida en = %i \n", RSTEP);
+ #endif
+ }
+void home()
+ {
+ vertex2d(0,0,0);
+ wait_ms(rap);
+ }
+void maxpos()
+ {
+ vertex2d(250,250,150);
+ }
+void punto(uint8_t x, uint8_t y)
+ {
+ command.printf("Coord x= %i, coord y=%i \n",x,y);
+ vertex2d(x,y,0);
+ wait_ms(1000);
+ vertex2d(x,y,11);
+ wait_ms(1000);
+ vertex2d(x,y,0);
+ wait_ms(1000);
+ home();
+ }
+
+void line(float xi, float xf, float yi, float yf)
+ {
+ // los parametros son coordenadas iniciales y finales de la linea en milimetros
+ #if DEBUG
+ Sdebug.printf("linea Xi= %f,Xf= %f,Yi= %f,Yf= %f \n", xi,xf,yi,yf);
+ #endif
+ float xp; float yp;
+ float m= (yf-yi)/(xf-xi);
+ float b=(yf-m*xf);
+ vertex2d(0,0,4);
+ wait(1);
+ vertex2d(xi,yi,abajo);
+ wait(1);
+ for (xp =xi;xp<=xf;xp+=RSTEP)
+ {
+ yp =m*xp+b;
+ vertex2d(xp,yp,abajo);
+ // savep( buffer_POS[INITPARAMETER==02]buffer_POS[INITPARAMETER+1==xp], buffer_POS[INITPARAMETER+2==xf],buffer_POS[INITPARAMETER+3==yp], buffer_POS[INITPARAMETER+4==yf]);
+
+ #if DEBUG
+ Sdebug.printf("coord X = %f,coord Y = %f \n", xp,yp);
+ #endif
+ }
+ nodraw();
+ home();
+ }
+ void rectangle(uint8_t xi, uint8_t yi,uint8_t a, uint8_t h)
+{
+ int x,y;
+ x = a+ xi;
+ y = h+ yi;
+
+ vertex2d(xi,yi,arriba);
+ wait_ms(600);
+ vertex2d(xi,yi,abajo);
+ wait_ms(600);
+ vertex2d(x,yi,abajo);
+ wait_ms(600);
+ vertex2d(x,y,abajo);
+ wait_ms(600);
+ vertex2d(xi,y,abajo);
+ wait_ms(600);
+ vertex2d(xi,yi,abajo);
+ wait_ms(600);
+ home();
+ }
+ void circle(uint8_t x1, uint8_t y1,uint8_t r)
+ {
+ double x2,y2,i,j;
+ vertex2d(x1 ,y1,arriba);
+ wait(1);
+ wait_ms(rap);
+
+ for ( x2=0;x2<=r;x2++){
+ y2= pow(((r*r)-(x2*x2)),0.5);
+ i= x2+x1;
+ j=y1+y2;
+ vertex2d(i ,j,abajo) ;
+ wait_ms(30);
+ #if DEBUG
+ Sdebug.printf("coord X = %f,coord Y = %f \n", i,j);
+ #endif
+ }
+
+ for ( x2=r;x2>=0;x2--){
+ y2= pow(((r*r)-(x2*x2)),0.5);
+ i= x2+x1;
+ j= y1 - y2;
+ vertex2d(i ,j,abajo) ;
+ wait_ms(30);
+ #if DEBUG
+ Sdebug.printf("coord X = %f,coord Y = %f \n", i,j);
+ #endif
+ }
+
+ for ( x2=0;x2<=r;x2++){
+ y2= pow(((r*r)-(x2*x2)),0.5);
+ i= x1- x2;
+ j= y1 - y2;
+ vertex2d(i ,j,abajo) ;
+ wait_ms(30);
+ #if DEBUG
+ Sdebug.printf("coord X = %f,coord Y = %f \n", i,j);
+ #endif
+ }
+ for ( x2=r;x2>=0;x2--){
+ y2= pow(((r*r)-(x2*x2)),0.5);
+ i= x1-x2;
+ j=y1+y2;
+ vertex2d(i ,j,abajo) ;
+ wait_ms(30);
+ #if DEBUG
+ Sdebug.printf("coord X = %f,coord Y = %f \n", i,j);
+ #endif
+ }
+ vertex2d(i ,j,arriba) ;
+ wait_ms(rap);
+ #if DEBUG
+ Sdebug.printf("coord X = %f,coord Y = %f \n", i,j);
+ #endif
+
+ if(x2==r)
+ {
+ wait(1);
+ }
+ home();
+ }
+
+
+
+ void savep(float pos){
+ for (uint8_t i=0; i<BUFF_POS;i++)
+ {
+ buffer_command[i]=command.getc();
+
+ }
+
+ }
+
+ void reanudar(){
+ led =0;
+ p=0;
+
+
+ }
+
+
+void pause(){
+ p=1;
+ led = !led;
+ timeout.attach(&pause, 300);
+ // while(1);
+
+
+ }
+
+void stop() //Funcion para detener el programa
+ {
+ exit(0);
+ home();
+ }
+
+void command_exe()
+ {
+ #if DEBUG
+ command.printf("Ejecutando comando: ");
+ #endif
+ switch (buffer_command[COMM_N])
+ {
+ case (LED):
+ #if DEBUG
+ command.printf("LED on\n");
+ #endif
+ command_led(buffer_command[INITPARAMETER]);
+ break;
+
+ case (PUNTO):
+ punto(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1]);
+ #if DEBUG
+ command.printf("Dibujando Punto");
+ #endif
+ break;
+
+ case LINE:
+ #if DEBUG
+ command.printf("Dibujando Linea\n");
+ #endif
+ line(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1],buffer_command[INITPARAMETER+2],buffer_command[INITPARAMETER+3]);
+
+
+ break;
+
+ case RECTANGLE:
+ #if DEBUG
+ command.printf("Dibujando Rectangulo\n");
+ #endif
+ rectangle(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1],buffer_command[INITPARAMETER+2],buffer_command[INITPARAMETER+3]);
+ break;
+
+ case (CIRCLE):
+ #if DEBUG
+ command.printf("Dibujando Circulo\n");
+ #endif
+ circle(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1],buffer_command[INITPARAMETER+2]);
+ break;
+
+ case (STOP):
+ #if DEBUG
+ command.printf(" STOP\n");
+ #endif
+ break;
+
+ case (PAUSE):
+ #if DEBUG
+ command.printf("programa pausado\n");
+ #endif
+ pause();
+ break;
+
+ case (REANUDAR):
+ #if DEBUG
+ command.printf("draw reanudar programa\n");
+ #endif
+ reanudar();
+ break;
+
+ case (RESOLUCION):
+ resolucion(buffer_command[INITPARAMETER]);
+ #if DEBUG
+ command.printf("Resolucion definida");
+ #endif
+ break;
+ case (HOME):
+ home();
+ #if DEBUG
+ command.printf("Posicion Home");
+ #endif
+ break;
+
+ case (TIEMPO_DE_PASO):
+ #if DEBUG
+ command.printf("Nuevo tiempo para paso:\res");
+ #endif
+ tiempo_de_paso(buffer_command[INITPARAMETER]);
+ break;
+
+ default:
+ #if DEBUG
+ command.printf("comando no encontrado\n");
+ #endif
+
+ }
+}
+
+
+
+
+
+
+
+
+
+
+int main() {
+ #if DEBUG
+ command.printf("inicio con debug\n");
+ #else
+ command.printf("inicio sin debug\n");
+ #endif
+ uint8_t val;
+ // mybutton.rise(&stop);
+ while(1){
+ val=command.getc();
+ if (val== '<'){
+ Read_command();
+ // ticker1.attach(&Read_command,05);
+ if (check_command()){
+ command_exe();
+ }
+ else{
+ #if DEBUG
+ command.printf("\n ERROR COMANDO -> ");
+ echo_command();
+ #endif
+ }
+ }
+ else{
+ #if DEBUG
+ command.printf("error de inicio de trama: ");
+ command.putc(val);
+ #endif
+ }
+
+ }
+
+
+ //ticker1.attach(&main,05);
+
+
+}
\ No newline at end of file