Pablo Lopez / Picoolo

picolo

Fork of Step_Motor by Steven Ortiz

Step_Motor.cpp@5:d92004b211c9, 2018-06-02 (annotated)

Committer:
pablolopez89
Date:
Sat Jun 02 16:34:11 2018 +0000
Revision:
5:d92004b211c9
Parent:
4:8581e6927b36 
WIKI Robot Piccolo

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Stevenor1997 0:ea300190c1f5 1 #include "Step_Motor.h"
Stevenor1997 0:ea300190c1f5 2 #include "mbed.h"
Stevenor1997 0:ea300190c1f5 3
pablolopez89 5:d92004b211c9 4 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) {
Stevenor1997 0:ea300190c1f5 5
Stevenor1997 0:ea300190c1f5 6
Stevenor1997 0:ea300190c1f5 7 motor_out=0x0;
Stevenor1997 0:ea300190c1f5 8 nstep=0;
pablolopez89 5:d92004b211c9 9 nstep2=0;
pablolopez89 5:d92004b211c9 10 motorSpeed=1500;
pablolopez89 5:d92004b211c9 11 add=0;
pablolopez89 5:d92004b211c9 12
Stevenor1997 0:ea300190c1f5 13 }
Stevenor1997 0:ea300190c1f5 14
Stevenor1997 0:ea300190c1f5 15
Stevenor1997 0:ea300190c1f5 16 void stepmotor::move() {
Stevenor1997 3:ec3add9f1351 17 switch(nstep2)
Stevenor1997 3:ec3add9f1351 18 {
pablolopez89 5:d92004b211c9 19 case 0: add = 0x1; break; // 0001
pablolopez89 5:d92004b211c9 20 case 1: add = 0x3; break; // 0011
pablolopez89 5:d92004b211c9 21 case 2: add = 0x2; break; // 0010
pablolopez89 5:d92004b211c9 22 case 3: add = 0x6; break; // 0110
pablolopez89 5:d92004b211c9 23 case 4: add = 0x4; break; // 0100
pablolopez89 5:d92004b211c9 24 case 5: add = 0xC; break; // 1100
pablolopez89 5:d92004b211c9 25 case 6: add = 0x8; break; // 1000
pablolopez89 5:d92004b211c9 26 case 7: add = 0x9; break; // 1001
Stevenor1997 3:ec3add9f1351 27
pablolopez89 5:d92004b211c9 28 default: add = 0x0; break; // 0000
Stevenor1997 3:ec3add9f1351 29 }
pablolopez89 5:d92004b211c9 30
Stevenor1997 0:ea300190c1f5 31 switch(nstep)
Stevenor1997 0:ea300190c1f5 32 {
pablolopez89 5:d92004b211c9 33 case 0: motor_out = 0x10 + add; break; // 0001 xxxx
pablolopez89 5:d92004b211c9 34 case 1: motor_out = 0x30 + add; break; // 0011 xxxx
pablolopez89 5:d92004b211c9 35 case 2: motor_out = 0x20 + add; break; // 0010 xxxx
pablolopez89 5:d92004b211c9 36 case 3: motor_out = 0x60 + add; break; // 0110 xxxx
pablolopez89 5:d92004b211c9 37 case 4: motor_out = 0x40 + add; break; // 0100 xxxx
pablolopez89 5:d92004b211c9 38 case 5: motor_out = 0xC0 + add; break; // 1100 xxxx
pablolopez89 5:d92004b211c9 39 case 6: motor_out = 0x80 + add; break; // 1000 xxxx
pablolopez89 5:d92004b211c9 40 case 7: motor_out = 0x90 + add; break; // 1001 xxxx
Stevenor1997 0:ea300190c1f5 41
pablolopez89 5:d92004b211c9 42 default: motor_out = 0x00 + add; break; // 0000 xxxx
Stevenor1997 0:ea300190c1f5 43 }
pablolopez89 5:d92004b211c9 44
Stevenor1997 0:ea300190c1f5 45 wait_us(motorSpeed);
Stevenor1997 0:ea300190c1f5 46
Stevenor1997 0:ea300190c1f5 47 }
Stevenor1997 0:ea300190c1f5 48
Stevenor1997 0:ea300190c1f5 49 void stepmotor::set_speed(int speed){
Stevenor1997 0:ea300190c1f5 50 motorSpeed=speed; //set motor speed us
Stevenor1997 0:ea300190c1f5 51 }
Stevenor1997 0:ea300190c1f5 52 uint32_t stepmotor::get_speed(){
Stevenor1997 0:ea300190c1f5 53 return motorSpeed; //
Stevenor1997 0:ea300190c1f5 54 }
Stevenor1997 0:ea300190c1f5 55
Stevenor1997 0:ea300190c1f5 56 void stepmotor::step(uint32_t num_steps, uint8_t cw) {
pablolopez89 5:d92004b211c9 57 //cw 0 si va a girar a la izquierda, 1 si gira a la derecha, 2 si sigue derecho, 3 retrocede
Stevenor1997 1:4c622abe2483 58
Stevenor1997 0:ea300190c1f5 59 uint32_t count=num_steps ;
Stevenor1997 0:ea300190c1f5 60 while(count){
pablolopez89 5:d92004b211c9 61 if (cw==1) {nstep++; nstep2--;}
pablolopez89 5:d92004b211c9 62 if (cw==0) {nstep--; nstep2++;}
pablolopez89 5:d92004b211c9 63 if (cw==2) {nstep++; nstep2++;}
pablolopez89 5:d92004b211c9 64 if (cw==3) {nstep--; nstep2--;}
Stevenor1997 0:ea300190c1f5 65 if (nstep>7) nstep=0;
Stevenor1997 0:ea300190c1f5 66 if (nstep<0) nstep=7;
pablolopez89 5:d92004b211c9 67 if (nstep2>7) nstep2=0;
pablolopez89 5:d92004b211c9 68 if (nstep2<0) nstep2=7;
Stevenor1997 0:ea300190c1f5 69 move();
Stevenor1997 0:ea300190c1f5 70 count--;
Stevenor1997 0:ea300190c1f5 71
Stevenor1997 0:ea300190c1f5 72 }
pablolopez89 5:d92004b211c9 73
Stevenor1997 0:ea300190c1f5 74 }