Speedy Retraction Team / Mbed 2 deprecated Nucleo_Dynamixel_18V

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   Dynamixel Protocol Utilities mbed

Fork of Nucleo_Dynamixel_18V by Timofey Ilin

main.cpp

Committer:
Mirthilon
Date:
2016-07-06
Revision:
3:09a27cf1c9fe
Parent:
2:05e91f425d43
Child:
4:0c1555b01361

File content as of revision 3:09a27cf1c9fe:

/*  Test    MX-28 ID: 0x01
    Pan     RX-28 ID: 0x02 (DMXL04)
    Tilt    RX-28 ID: 0x03 (DMXL05)
    Base    MX-28 ID: 0x04
    Elbow   RX-28 ID: 0x05 (DMXL06)
    Wrist   RX-28 ID: 0x06 (DMXL08)
    //DON'T GO TO ZERO ON BASE SERVO
*/

#include "mbed.h"
#include "Dynamixel.h"
Serial pc(USBTX,USBRX);
Dynamixel Base(PA_9, PA_10, PB_3, 1, 57600);
Dynamixel Elbow(PA_9, PA_10, PB_3, 5, 57600);
Dynamixel Pan(PA_9, PA_10, PB_3, 4, 57600);
Dynamixel Tilt(PA_9, PA_10, PB_3, 3, 57600);
DigitalOut myled(LED1);
uint8_t pos;
uint16_t tiltInc = 15;
uint16_t panInc = 60;
int echo;

void centrePosition() {
    Base.setGoalPosition(800);
    Elbow.setGoalPosition(50);
    pos = 2;
    wait(2);
}

void leftPosition() {
    uint16_t currentPosBase;
    uint16_t currentPosElbow;
    
    if (pos == 1) {
        centrePosition();
        currentPosBase = Base.getPresentPosition();
        currentPosElbow = Elbow.getPresentPosition();
        while(currentPosBase > 820 && currentPosElbow > 70) {
            currentPosBase = Base.getPresentPosition();
            currentPosElbow = Elbow.getPresentPosition();
        }
    }
    Base.setGoalPosition(300);
    Elbow.setGoalPosition(50);
    pos = 3;
    wait(2);
}

void rightPosition() {
    uint16_t currentPosBase;
    uint16_t currentPosElbow;
    
    if (pos == 1) {
        centrePosition();
        currentPosBase = Base.getPresentPosition();
        currentPosElbow = Elbow.getPresentPosition();
        while(currentPosBase > 820 && currentPosElbow > 70) {
            currentPosBase = Base.getPresentPosition();
            currentPosElbow = Elbow.getPresentPosition();
        }
    }
    Base.setGoalPosition(1300);
    Elbow.setGoalPosition(50);
    pos = 4;
    wait(2);
}

void retractedPosition() {
    uint16_t currentPosBase;
    
    if (pos == 3) {
        centrePosition();
        currentPosBase = Base.getPresentPosition();
        while(currentPosBase < 780) {
            currentPosBase = Base.getPresentPosition();
        }
    }
    else if (pos == 4) {
        centrePosition();
        currentPosBase = Base.getPresentPosition();
        while(currentPosBase > 820) {
            currentPosBase = Base.getPresentPosition();
        }
    }
    Base.setGoalPosition(1300);
    Elbow.setGoalPosition(365);
    pos = 1;
    wait(2);
}

void measureTime() {
    uint16_t currentPosBase;
    Timer t;
    uint32_t time;
    
    t.start();
    currentPosBase = Base.getPresentPosition();
    while(currentPosBase < 1280) {
        currentPosBase = Base.getPresentPosition();
    }
    time = t.read_ms();
    pc.printf("Time: %d\r\n",time);
    t.stop();
    t.reset();
}

void setSpeed(uint16_t speed) {
    Base.setMovingSpeed(1024+speed);
    Elbow.setMovingSpeed(1024+2.2*speed);
}

void setLimits() {
    Base.setCWAngleLimit(300);
    Elbow.setCWAngleLimit(50);
    Base.setCCWAngleLimit(1300);
    Elbow.setCCWAngleLimit(365);
    Tilt.setCWAngleLimit(210);
    Tilt.setCCWAngleLimit(1000);
}

void tiltIncrementUp() {
    uint16_t currentPosTilt;
    
    currentPosTilt = Tilt.getPresentPosition();
    Tilt.setGoalPosition(currentPosTilt+tiltInc);
    wait_ms(500);
}

void tiltIncrementDown() {
    uint16_t currentPosTilt;
    
    currentPosTilt = Tilt.getPresentPosition();
    Tilt.setGoalPosition(currentPosTilt-tiltInc);
    wait_ms(500);
}

void panIncrementLeft() {
    uint16_t currentPosPan;
    
    currentPosPan = Pan.getPresentPosition();
    Pan.setMovingSpeed(1124);
    Pan.setGoalPosition(4095);
}

void panIncrementRight() {
    uint16_t currentPosPan;
    
    currentPosPan = Pan.getPresentPosition();
    Pan.setMovingSpeed(1124);
    Pan.setGoalPosition(0);
}
 
int main()
{
    pc.baud(9600);
    //setSpeed(200);
    uint16_t currentPosTilt;
    uint16_t currentPosPan;
    Pan.setCWAngleLimit(0);
    Pan.setCCWAngleLimit(4095);
    Pan.setGoalPosition(500);
    Tilt.setGoalPosition(700);
    
    while(1) {
        //if (echo == 0) {  
            //Base.setGoalPosition(1300);
            //Elbow.setGoalPosition(365);
            
        //}
         
        echo = pc.getc();
        pc.printf("\r\n %d \r\n", echo);
        pc.putc(echo);
        /*if (echo == 49) {    retractedPosition();    }
        else if (echo == 50) {    centrePosition();  }
        else if (echo == 51) {    leftPosition();    }
        else if (echo == 52) {    rightPosition();   }*/
        if (echo == 119) {   tiltIncrementUp();   }
        else if (echo == 115) {   tiltIncrementDown();   }
        else if (echo == 97) {    panIncrementLeft();   }
        else if (echo == 100) {   panIncrementRight();   }
        else {  
            Tilt.setMovingSpeed(1024);
            Pan.setMovingSpeed(1024);
            currentPosPan = Pan.getPresentPosition();
            Pan.setGoalPosition(currentPosPan);
        }
        echo = 0;
        echo = pc.getc();
        pc.printf("\r\n %d \r\n", echo);
        pc.putc(echo);
        
    
        currentPosTilt = Tilt.getPresentPosition();
        //pc.printf("Tilt Position: %d\r\n",currentPosTilt);
        currentPosPan = Pan.getPresentPosition();
        //pc.printf("Pan Position: %d\r\n",currentPosPan);
        
            
        /*
        retractedPosition();
        wait(3);
        leftPosition();
        wait(3);
        rightPosition();
        wait(3);
        centrePosition();
        wait(3);
        retractedPosition();
        wait(3);
        rightPosition();
        wait(3);
        centrePosition();
        wait(3);
        leftPosition();
        wait(3);*/
    }
    
        
}

/*//MX28a.reset();
        MX28a.setId(6);
        //wait(1);
        MX28a.setEnableLED(1);
        //RX28a.setEnableLED(0);
        wait(1);
        MX28a.setEnableLED(0);
        //RX28a.setEnableLED(1);
        wait(1);
        MX28a.setGoalPosition(0);
        RX28a.setGoalPosition(1023);
        wait(1);
        MX28a.setGoalPosition(4095);
        RX28a.setGoalPosition(0);
        wait(1);
        delayTest = MX28a.getReturnDelayTime();
        pc.printf("Delay Time: 0x%04x\r\n",delayTest);
        wait(1);
        modelNumber = MX28a.getModelNumber();
        pc.printf("Model Number: 0x%04x\r\n",modelNumber);
        wait(1);
        id = MX28a.getId();
        pc.printf("ID Number: 0x%04x\r\n",id);
        if (id == 0x01) {
            myled = !myled;
        }
        wait(1);
        baud = MX28a.getBaudRate();
        pc.printf("Baud Rate: %d\r\n",baud);
        
        
        */
/*int rar = 1;
int goalPos = 60;

int Xm = 0;
int Ym = 0;
int Xe = 0;
int Ye = 0;

int L = 147; // mm
int S = 97; // mm
int thet = 0;
int gamm = 0;*/

/*currentPosBase = Base.getPresentPosition();
        pc.printf("Base Current Position: %d\r\n",currentPosBase);
        currentPosElbow = Elbow.getPresentPosition();
        pc.printf("Elbow Current Position: %d\r\n",currentPosElbow);
        //wait(3);
        if (currentPosElbow == 360 && rar == 1) {
            rar = -1;
        }
        else if (currentPosElbow == 60 && rar == -1) {
            rar = 1;
        }
        Elbow.setGoalPosition(goalPos);
        
        goalPos += rar;*/
        //error = Base.setGoalPosition(0);
        //Elbow.setGoalPosition(0);
        //pc.printf("Error: 0x%04x\r\n",error);
        //wait(1);