18 servos moving randomly using rcservo_EnterPlayMode_NOFB

[roboard]

I don't think the error is in the initializing in my code it only happens at the rcservo_MoveTo Command.

Hi,

we have test 18 KONDO KRS-786 servos on RB-100 & RoBoIO 1.61 with the same channels: channel 0, 1, 2, 3, 4, 5, 9, 10, 11, 13, 14, 15, 16, 17, 18, 21, 22, 23. And rcservo_MoveTo() works normally; so it may not be the issue of RoBoIO.

The following steps may help you to debug:

1.
print

LF.Coxa.Pin + LF.Femur.Pin + LF.Tibia.Pin +
LM.Coxa.Pin + LM.Femur.Pin + LM.Tibia.Pin +
LR.Coxa.Pin + LR.Femur.Pin + LR.Tibia.Pin +
RF.Coxa.Pin + RF.Femur.Pin + RF.Tibia.Pin +
RM.Coxa.Pin + RM.Femur.Pin + RM.Tibia.Pin +
RR.Coxa.Pin + RR.Femur.Pin + RR.Tibia.Pin

to see whether it is equal to 0x00e7ee3f.

2.
print each element of motion[32], before calling rcservo_MoveTo(), to see whether it is a valid PWM width for your servos.

3.
run only 2 or 3 servos with your 18 servo code to see whether it works.

[dragonax]

Hi,

1.

I printed

LF.Coxa.Pin + LF.Femur.Pin + LF.Tibia.Pin +
LM.Coxa.Pin + LM.Femur.Pin + LM.Tibia.Pin +
LR.Coxa.Pin + LR.Femur.Pin + LR.Tibia.Pin +
RF.Coxa.Pin + RF.Femur.Pin + RF.Tibia.Pin +
RM.Coxa.Pin + RM.Femur.Pin + RM.Tibia.Pin +
RR.Coxa.Pin + RR.Femur.Pin + RR.Tibia.Pin

and it comes out as e7ee3f

2.

I checked each element of motion and all of them are between 1000- 1800

3. tried with 2 servos connected and the 18 servo code moves them randomly.


it seems like if the servo value is set as 1600 then it moves from around 1300 - 1900 really fast.


I even tried with 2 new 645mg servos still same problem

btw I am running the code on ubuntu. Also tried without the gnome (in command line) still no luck.

Thanks

[roboard]

Please check the following two lines in your function ServoDriver():

Code: Select all

rcservo_EnterPlayMode_NOFB(motion);
rcservo_MoveTo(motion, 1000L);


The first line calls rcservo_EnterPlayMode_NOFB(motion), which will move all servos to the positions defined by motion in the fastest speed. This can make servos look like moving randomly.

The second line calls rcservo_MoveTo(motion, 1000L) to move all servos again to the same positions defined by motion, which actually takes no effect.

The following steps may help you to debug further:

1.
Please check whether the random move is caused when calling rcservo_EnterPlayMode_NOFB().

2.
After calling rcservo_EnterPlayMode_NOFB(), please check whether your function TripodGait() works normally.

[foe.pexal]

Thanks here is the code I have so far

Code: Select all


#include <iostream>
#include <roboard>
#include <rcservo>
#include <string>
#include <stdio>             

struct servo {
    const int Pin;            // servo port
    const int PinNum;         // servo port
    const int Center;         // center point in Degrees
    double Angle;             // actuall angle
};

struct leg {
    struct servo Coxa;
    struct servo Femur;
    struct servo Tibia;
};

struct leg LF = {{ RCSERVO_USECHANNEL23,23,-2,0},{ RCSERVO_USECHANNEL22,22,0,0},{ RCSERVO_USECHANNEL21,21,-45,0}};
struct leg LM = {{ RCSERVO_USECHANNEL9,9, -19,0},{ RCSERVO_USECHANNEL10,10,-1,0},{ RCSERVO_USECHANNEL11,11,-40,0}};
struct leg LR = {{ RCSERVO_USECHANNEL15,15,-30,0},{ RCSERVO_USECHANNEL13,13,-8,0},{ RCSERVO_USECHANNEL14,14,-37,0}};

struct leg RF = {{ RCSERVO_USECHANNEL18,18,0,0},{ RCSERVO_USECHANNEL17,17,7,0},{ RCSERVO_USECHANNEL16,16,37,0}};
struct leg RM = {{ RCSERVO_USECHANNEL4,4, 7,0},{ RCSERVO_USECHANNEL20,20, 8,0},{ RCSERVO_USECHANNEL2,2, 30,0}};
struct leg RR = {{ RCSERVO_USECHANNEL1,1, 30,0},{ RCSERVO_USECHANNEL3,3, 16,0},{ RCSERVO_USECHANNEL0,0, 38,0}};

unsigned long motion[32];

using namespace std;


int Angle2ServoPos(double angle){

    //result 500-2500
    //angle in degrees(-90 to +90) 0 = center
    return (int)((angle * 2000/180) + 1500);

}

void ServoDriver(){


    //Set servo type here first
    /*if (rcservo_SetServos(
    RCSERVO_USECHANNEL23 + RCSERVO_USECHANNEL22 + RCSERVO_USECHANNEL21 +
    RCSERVO_USECHANNEL18 + RCSERVO_USECHANNEL17 + RCSERVO_USECHANNEL16 +
    RCSERVO_USECHANNEL9 + RCSERVO_USECHANNEL10 + RCSERVO_USECHANNEL11 +
    RCSERVO_USECHANNEL4 + RCSERVO_USECHANNEL5 + RCSERVO_USECHANNEL2 +
    RCSERVO_USECHANNEL15 + RCSERVO_USECHANNEL13 + RCSERVO_USECHANNEL14 +
    RCSERVO_USECHANNEL1+RCSERVO_USECHANNEL3+RCSERVO_USECHANNEL0 ,
        RCSERVO_SERVO_DEFAULT_NOFB)){*/

    if (rcservo_SetServos(
        LF.Coxa.Pin + LF.Femur.Pin + LF.Tibia.Pin +
        LM.Coxa.Pin + LM.Femur.Pin + LM.Tibia.Pin +
        LR.Coxa.Pin + LR.Femur.Pin + LR.Tibia.Pin +
        RF.Coxa.Pin + RF.Femur.Pin + RF.Tibia.Pin +
        RM.Coxa.Pin + RM.Femur.Pin + RM.Tibia.Pin +
        RR.Coxa.Pin + RR.Femur.Pin + RR.Tibia.Pin ,
        RCSERVO_SERVO_DEFAULT_NOFB)){
        //Initialize servo now
        if (rcservo_Initialize(
            LF.Coxa.Pin + LF.Femur.Pin + LF.Tibia.Pin +
            LM.Coxa.Pin + LM.Femur.Pin + LM.Tibia.Pin +
            LR.Coxa.Pin +
            LR.Femur.Pin +
            //LR.Tibia.Pin +   //If I comment any one of the pins it works fine
            RF.Coxa.Pin + RF.Femur.Pin + RF.Tibia.Pin +
            RM.Coxa.Pin + RM.Femur.Pin + RM.Tibia.Pin +
            RR.Coxa.Pin + RR.Femur.Pin + RR.Tibia.Pin  )){

            //sleep(1);


            rcservo_EnterPlayMode_NOFB(motion);


            rcservo_MoveTo(motion, 1000L);

            //rcservo_SetAction(motion, 2000L);
            //while (rcservo_PlayAction()!= RCSERVO_PAUSED){
                //rcservo_PauseAction();

            //};


            //rcservo_Close();
            //}else

        }
        else
        {
            //On error initialize servo
            printf("\n\nError: %s", roboio_GetErrMsg());
        };
    }
    else
    {
        //On error setting servo type
        printf("\n\nError: %s", roboio_GetErrMsg());
    };
};

void MoveServos(){
   rcservo_MoveTo(motion, 400L);
}

void SetHomePosition(){
    RF.Coxa.Angle=RF.Coxa.Center+30;
    RM.Coxa.Angle=RM.Coxa.Center;
    RR.Coxa.Angle=RR.Coxa.Center-30;

    RF.Femur.Angle=RF.Femur.Center+30;
    RM.Femur.Angle=RM.Femur.Center+30;
    RR.Femur.Angle=RR.Femur.Center+30;

    RF.Tibia.Angle=RF.Tibia.Center+33;
    RM.Tibia.Angle=RM.Tibia.Center+33;
    RR.Tibia.Angle=RR.Tibia.Center+33;

    LF.Coxa.Angle=LF.Coxa.Center-30;
    LM.Coxa.Angle=LM.Coxa.Center;
    LR.Coxa.Angle=LR.Coxa.Center+30;

    LF.Femur.Angle=LF.Femur.Center-30;
    LM.Femur.Angle=LM.Femur.Center-30;
    LR.Femur.Angle=LR.Femur.Center-30;

    LF.Tibia.Angle=LF.Tibia.Center-33;
    LM.Tibia.Angle=LM.Tibia.Center-33;
    LR.Tibia.Angle=LR.Tibia.Center-33;

};

void CalServoPositions(){
    motion[RF.Coxa.PinNum]=Angle2ServoPos(RF.Coxa.Angle);   
    motion[RM.Coxa.PinNum]=Angle2ServoPos(RM.Coxa.Angle);   
    motion[RR.Coxa.PinNum]=Angle2ServoPos(RR.Coxa.Angle);   

    motion[RF.Femur.PinNum]=Angle2ServoPos(RF.Femur.Angle);   
    motion[RM.Femur.PinNum]=Angle2ServoPos(RM.Femur.Angle);   
    motion[RR.Femur.PinNum]=Angle2ServoPos(RR.Femur.Angle);   

    motion[RF.Tibia.PinNum]=Angle2ServoPos(RF.Tibia.Angle);   
    motion[RM.Tibia.PinNum]=Angle2ServoPos(RM.Tibia.Angle); 
    motion[RR.Tibia.PinNum]=Angle2ServoPos(RR.Tibia.Angle);   

    motion[LF.Coxa.PinNum]=Angle2ServoPos(LF.Coxa.Angle);   
    motion[LM.Coxa.PinNum]=Angle2ServoPos(LM.Coxa.Angle);   
    motion[LR.Coxa.PinNum]=Angle2ServoPos(LR.Coxa.Angle);   

    motion[LF.Femur.PinNum]=Angle2ServoPos(LF.Femur.Angle); 
    motion[LM.Femur.PinNum]=Angle2ServoPos(LM.Femur.Angle); 
    motion[LR.Femur.PinNum]=Angle2ServoPos(LR.Femur.Angle);   

    motion[LF.Tibia.PinNum]=Angle2ServoPos(LF.Tibia.Angle); 
    motion[LM.Tibia.PinNum]=Angle2ServoPos(LM.Tibia.Angle); 
    motion[LR.Tibia.PinNum]=Angle2ServoPos(LR.Tibia.Angle);   
};

void RippleGait(){

    //Move RF and LR forward
    RF.Femur.Angle = RF.Femur.Angle + 20;
    LR.Femur.Angle = LR.Femur.Angle - 20;
    CalServoPositions();
    MoveServos();

    RF.Coxa.Angle = RF.Coxa.Angle +20;
    RF.Tibia.Angle = RF.Tibia.Angle -20; //- expands
    LR.Coxa.Angle = LR.Coxa.Angle -20;
    LR.Tibia.Angle = LR.Tibia.Angle +20; //- expands
    CalServoPositions();
    MoveServos();

    RF.Femur.Angle = RF.Femur.Center+30;
    RF.Tibia.Angle = RF.Tibia.Center+33; //- expands
    LR.Femur.Angle = LR.Femur.Center-30;
    LR.Tibia.Angle = LR.Tibia.Center-33; //- expands
    CalServoPositions();
    MoveServos();

    RF.Coxa.Angle = RF.Coxa.Center+30;
    LR.Coxa.Angle = LR.Coxa.Center+30;

    RM.Coxa.Angle = RM.Coxa.Center+10;
    RR.Coxa.Angle = RR.Coxa.Center+10;
    LF.Coxa.Angle = LF.Coxa.Center-10;
    LM.Coxa.Angle = LM.Coxa.Center-10;
    CalServoPositions();
    MoveServos();


    //Move RR and LM forward
    RR.Femur.Angle = RR.Femur.Angle + 20;
    LM.Femur.Angle = LM.Femur.Angle - 20;
    CalServoPositions();
    MoveServos();

    RR.Coxa.Angle = RR.Coxa.Angle +20;
    RR.Tibia.Angle = RR.Tibia.Angle -20; //- expands
    LM.Coxa.Angle = LM.Coxa.Angle -20;
    LM.Tibia.Angle = LM.Tibia.Angle +20; //- expands
    CalServoPositions();
    MoveServos();


    RR.Femur.Angle = RR.Femur.Center+30;
    RR.Tibia.Angle = RR.Tibia.Center+33; //- expands
    LM.Femur.Angle = LM.Femur.Center-30;
    LM.Tibia.Angle = LM.Tibia.Center-33; //- expands
    CalServoPositions();
    MoveServos();

    RR.Coxa.Angle = RR.Coxa.Center-30;
    LM.Coxa.Angle = LM.Coxa.Center;
    CalServoPositions();
    MoveServos();



    //Move RM and LF forward
    RM.Femur.Angle = RM.Femur.Angle + 20;
    LF.Femur.Angle = LF.Femur.Angle - 20;
    CalServoPositions();
    MoveServos();

    RM.Coxa.Angle = RM.Coxa.Angle +20;
    RM.Tibia.Angle = RM.Tibia.Angle -20; //- expands
    LF.Coxa.Angle = LF.Coxa.Angle -20;
    LF.Tibia.Angle = LF.Tibia.Angle +20; //- expands
    CalServoPositions();
    MoveServos();


    RM.Femur.Angle = RM.Femur.Center+30;
    RM.Tibia.Angle = RM.Tibia.Center+33; //- expands
    LF.Femur.Angle = LF.Femur.Center-30;
    LF.Tibia.Angle = LF.Tibia.Center-33; //- expands
    CalServoPositions();
    MoveServos();

    RM.Coxa.Angle = RM.Coxa.Center;
    LF.Coxa.Angle = LF.Coxa.Center-30;
    CalServoPositions();
    MoveServos();
};

void TripodGait(){
//Move RF & LM & RR forward
    RF.Femur.Angle = RF.Femur.Angle + 20;
    LM.Femur.Angle = LM.Femur.Angle - 20;
    RR.Femur.Angle = RR.Femur.Angle + 20;
    CalServoPositions();
    MoveServos();

    RF.Coxa.Angle = RF.Coxa.Angle +20;
    RF.Tibia.Angle = RF.Tibia.Angle -20; //- expands
    LM.Coxa.Angle = LM.Coxa.Angle -20;
    LM.Tibia.Angle = LM.Tibia.Angle +20; //- expands
    RR.Coxa.Angle = RR.Coxa.Angle +20;
    RR.Tibia.Angle = RR.Tibia.Angle -20; //- expands
    CalServoPositions();
    MoveServos();


    RF.Femur.Angle = RF.Femur.Center+30;
    RF.Tibia.Angle = RF.Tibia.Center+33; //- expands
    LM.Femur.Angle = LM.Femur.Center-30;
    LM.Tibia.Angle = LM.Tibia.Center-33; //- expands
    RR.Femur.Angle = RR.Femur.Center+30;
    RR.Tibia.Angle = RR.Tibia.Center+33; //- expands
    CalServoPositions();
    MoveServos();

    RF.Coxa.Angle = RF.Coxa.Center+30;
    LM.Coxa.Angle = LM.Coxa.Center;
    RR.Coxa.Angle = RR.Coxa.Center-30;

    LF.Femur.Angle = LF.Femur.Angle - 20;
    RM.Femur.Angle = RM.Femur.Angle + 20;
    LR.Femur.Angle = LR.Femur.Angle - 20;
    CalServoPositions();
    MoveServos();




   //Move LF & RM & LR forward



    LF.Coxa.Angle = LF.Coxa.Angle -20;
    LF.Tibia.Angle = LF.Tibia.Angle +20; //- expands
    RM.Coxa.Angle = RM.Coxa.Angle +20;
    RM.Tibia.Angle = RM.Tibia.Angle -20; //- expands
    LR.Coxa.Angle = LR.Coxa.Angle -20;
    LR.Tibia.Angle = LR.Tibia.Angle +20; //- expands
    CalServoPositions();
    MoveServos();


    LF.Femur.Angle = LF.Femur.Center-30;
    LF.Tibia.Angle = LF.Tibia.Center-33; //- expands
    RM.Femur.Angle = RM.Femur.Center+30;
    RM.Tibia.Angle = RM.Tibia.Center+33; //- expands
    LR.Femur.Angle = LR.Femur.Center-30;
    LR.Tibia.Angle = LR.Tibia.Center-33; //- expands
    CalServoPositions();
    MoveServos();

    LF.Coxa.Angle = LF.Coxa.Center-30;
    RM.Coxa.Angle = RM.Coxa.Center;
    LR.Coxa.Angle = LR.Coxa.Center+30;
    CalServoPositions();
    MoveServos();
};


int main()
{

    //set roboard version in our case its roboard 100
    roboio_SetRBVer(RB_100);
    SetHomePosition();
    CalServoPositions();
    ServoDriver();

   //do 4 steps
    for (int i=0;i<3;i++){
        TripodGait();
    }




    return 0;
}


I have using this source code to Roboard RB-110, but why the servos is getting hot ?

[dragonax]

I see that you are using RB-110. If im not mistaken RB-110 has only 18 serov ports and in the code you have used ports above 18 are specified. Try changing the code

[foe.pexal]

yes, that's right. but I have already changing the code with just one servo to control the speed of servo and the servo still getting hot if i run the program. what should I do?

[dragonax]

Today I got 12 digital servos and 6 analogue servos and again I got the same problem of them all moving randomly. I managed to find a temp fix for it by adding in an extra servo so now I have total of 19 servos connected but only 18 of them are initialized. As long as the 19th servo is connect and not initialized the other 18 works fine.

Strange solution I know