Step_Motor.cpp

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