abrayan

Dependencies:   mbed

mover.cpp

Committer:
angel123
Date:
7 months ago
Revision:
6:8d7f6fe73ed1
Parent:
2:3007b3c06d2c

File content as of revision 6:8d7f6fe73ed1:


#include "mover.h"
#include "mbed.h"
#include "math.h"


PwmOut myServo1(PA_10);
PwmOut myServo2(PB_3);
PwmOut myServo3(PB_5);
PwmOut myServo4(PB_4);
PwmOut myServo5(PB_10);
PwmOut myServo6(PA_8);
PwmOut myServo7(PB_6);
PwmOut myServo8(PA_7);



uint8_t  ss_time=180;     // tiempo  de espera para moverse 1 mm en microsegundos

void put_sstime(uint8_t vtime){
    ss_time=vtime;
    
}

int coord2us(float coord)
{
    if(0 <= coord <= MAXPOS)
        return int(750+coord*1900/180);// u6
    return 750;

}


 


void mover_ser(uint8_t motor, uint8_t pos){

    int pulseX = coord2us(pos);
    
    switch (motor)
    {
        case 0x01:  myServo1.pulsewidth_us(pulseX);
                    break;
                    
        case 0x02:  myServo2.pulsewidth_us(pulseX);
                    break;            
    
        case 0x03:  myServo3.pulsewidth_us(pulseX);
                    break;
                    
        case 0x04:  myServo4.pulsewidth_us(pulseX);
                    break;
                    
        case 0x05:  myServo5.pulsewidth_us(pulseX);
                    break;
                    
        case 0x06:  myServo6.pulsewidth_us(pulseX);
                    break;                    
                    
        case 0x07:  myServo7.pulsewidth_us(pulseX);
                    break;
                    
        case 0x08:  myServo8.pulsewidth_us(pulseX);
                    break;                    
    }
}

void init_servo()
{
    myServo1.period_ms(20);
    myServo2.period_ms(20);
    myServo3.period_ms(20);
    myServo4.period_ms(20);
    myServo5.period_ms(20);
    myServo6.period_ms(20);
    myServo7.period_ms(20);
    myServo8.period_ms(20); 
}