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:
daemonslayer
Date:
2016-07-05
Revision:
1:bae7d30014b7
Parent:
0:9e2ed7d4d2ea
Child:
2:05e91f425d43

File content as of revision 1:bae7d30014b7:

/*  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, 4, 57600);
Dynamixel Elbow(PA_9, PA_10, PB_3, 5, 57600);
DigitalOut myled(LED1);
uint8_t pos;

void centrePosition() {
    Base.setGoalPosition(800);
    Elbow.setGoalPosition(50);
    pos = 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;
}

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;
}

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;
}

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);
}
 
int main()
{
    pc.baud(9600);
    setSpeed(200);
    
    while(1) {
        retractedPosition();
        wait(2);
        leftPosition();
        wait(2);
        rightPosition();
        wait(2);
        centrePosition();
        wait(2);
        retractedPosition();
        wait(2);
        rightPosition();
        wait(2);
        centrePosition();
        wait(2);
        leftPosition();
        wait(2);
    }
        
}

/*//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 Xm = 0;
int Ym = 0;
int Xe = 0;
int Ye = 0;

int L = 147; // mm
int S = 97; // mm

int theta = 370;   // Base angle
int gamma = 60;   // Elbow angle


//As Xe increases, alter Xm and Ym to keep Ye at 0
while (1) {
    if (Xm > 50) {
        Xm += 1;
        theta = arccos(Xm/L);
        gamma = arccos(Xm/S);
    }
}
        
        */
/*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);