Step_Motor.cpp

00001 #include "Step_Motor.h"
00002 #include "mbed.h"
00003 #include "General.h"
00004  
00005  
00006  Serial pc1(USBTX,USBRX);
00007  
00008     
00009 stepmotor::stepmotor(PinName in1, PinName in2, PinName in3, PinName in4,PinName in5, PinName in6, PinName in7, PinName in8) : motor_out(in1,in2,in3,in4,in5,in6,in7,in8) { 
00010  
00011  
00012     motor_out=0x0;
00013     nstep=0;
00014     motorSpeed=1200;
00015     nstep2=7;
00016 }
00017  
00018  
00019 void stepmotor::move() { 
00020    
00021    switch(nstep2)
00022         { 
00023             case 0: ms2 = 0x1;  break;  // 0001 XXXX
00024             case 1: ms2 = 0x3; break;  // 0011
00025             case 2: ms2 = 0x2;  break;  // 0010   
00026             case 3: ms2 = 0x6;  break;  // 0110
00027             case 4: ms2 = 0x4;  break;  // 0100
00028             case 5: ms2 = 0xC;  break;  // 1100
00029             case 6: ms2 = 0x8;  break;  // 1000
00030             case 7: ms2 = 0x9;  break;  // 1001
00031             
00032             default: ms2 = 0x0; break; // 0000
00033         }
00034    
00035    switch(nstep)
00036         { 
00037             case 0: motor_out = 0x10 + ms2;  break;  // 0001 XXXX
00038             case 1: motor_out = 0x30 + ms2; break;  // 0011
00039             case 2: motor_out = 0x20 + ms2;  break;  // 0010   
00040             case 3: motor_out = 0x60 + ms2;  break;  // 0110
00041             case 4: motor_out = 0x40 + ms2;  break;  // 0100
00042             case 5: motor_out = 0xC0 + ms2;  break;  // 1100
00043             case 6: motor_out = 0x80 + ms2;  break;  // 1000
00044             case 7: motor_out = 0x90 + ms2;  break;  // 1001
00045             
00046             default: motor_out = 0x00 + ms2; break; // 0000
00047         }
00048         wait_us(motorSpeed);
00049         
00050 }
00051  
00052 void stepmotor::set_speed(int speed){
00053     motorSpeed=speed; //set motor speed us 
00054 }
00055 uint32_t stepmotor::get_speed(){
00056     return motorSpeed; // 
00057 }
00058  
00059 void stepmotor::step(uint32_t num_steps, uint8_t cw) {
00060 // funcion para mover el motor N pasos CW o CCW   
00061 // num_steps  número de paso que da el motor 
00062 // cw =True  para dirección en sentido del reloj 
00063 // cw =False para dirección contraria de las manecillas del reloj         
00064     pc1.baud(9600);              //programar los baudios
00065     pc1.format(8,SerialBase::None,1); 
00066     
00067     uint32_t count=num_steps ;
00068     while(count){
00069         if (cw==DER)   nstep++; nstep2++;    
00070         if (cw==IZQ)  nstep--; nstep2--;
00071         if (cw==UP)   nstep++; nstep2--;    
00072         if (cw==DOWN)  nstep--; nstep2++;  
00073         if (nstep>7) nstep=0;
00074         if (nstep<0) nstep=7;
00075         if (nstep2>7) nstep=0;
00076         if (nstep2<0) nstep=7;
00077         move();
00078         count--;
00079  
00080     }
00081     if(cw==UP) pc1.printf("Avance \n");
00082     else if (cw==DOWN) pc1.printf("Retrocedi \n");
00083     else if (cw==IZQ) pc1.printf("Gire Izq \n");
00084     else if (cw==DER) pc1.printf("Gire Der \n");
00085 }