Step_Motor.cpp

00001 #include "Step_Motor.h"
00002 #include "mbed.h"
00003  
00004 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) { 
00005  
00006  
00007     motor_out=0x0;
00008     nstep=0;
00009     nstep2=0;
00010     motorSpeed=1500;
00011     add=0;
00012     
00013 }
00014  
00015  
00016 void stepmotor::move() { 
00017    switch(nstep2)
00018         { 
00019             case 0: add = 0x1; break;  // 0001
00020             case 1: add = 0x3; break;  // 0011
00021             case 2: add = 0x2; break;  // 0010   
00022             case 3: add = 0x6; break;  // 0110
00023             case 4: add = 0x4; break;  // 0100
00024             case 5: add = 0xC; break;  // 1100
00025             case 6: add = 0x8; break;  // 1000
00026             case 7: add = 0x9; break;  // 1001
00027             
00028             default: add = 0x0; break; // 0000
00029         }
00030         
00031    switch(nstep)
00032         { 
00033             case 0: motor_out = 0x10 + add; break;  // 0001 xxxx
00034             case 1: motor_out = 0x30 + add; break;  // 0011 xxxx
00035             case 2: motor_out = 0x20 + add; break;  // 0010 xxxx  
00036             case 3: motor_out = 0x60 + add; break;  // 0110 xxxx
00037             case 4: motor_out = 0x40 + add; break;  // 0100 xxxx
00038             case 5: motor_out = 0xC0 + add; break;  // 1100 xxxx
00039             case 6: motor_out = 0x80 + add; break;  // 1000 xxxx
00040             case 7: motor_out = 0x90 + add; break;  // 1001 xxxx
00041             
00042             default: motor_out = 0x00 + add; break; // 0000 xxxx
00043         }
00044         
00045         wait_us(motorSpeed);
00046         
00047 }
00048  
00049 void stepmotor::set_speed(int speed){
00050     motorSpeed=speed; //set motor speed us 
00051 }
00052 uint32_t stepmotor::get_speed(){
00053     return motorSpeed; // 
00054 }
00055  
00056 void stepmotor::step(uint32_t num_steps, uint8_t cw) {
00057 //cw 0 si va a girar a la izquierda, 1 si gira a la derecha, 2 si sigue derecho, 3 retrocede      
00058     
00059     uint32_t count=num_steps ;
00060     while(count){
00061         if (cw==1)   {nstep++; nstep2--;}    
00062         if (cw==0)   {nstep--; nstep2++;}
00063         if (cw==2)   {nstep++;  nstep2++;}
00064         if (cw==3)   {nstep--;  nstep2--;}
00065         if (nstep>7) nstep=0;
00066         if (nstep<0) nstep=7;
00067         if (nstep2>7) nstep2=0;
00068         if (nstep2<0) nstep2=7;
00069         move();
00070         count--;
00071  
00072     }
00073   
00074 }