mechatronics / robotics
Blog
  • HOME »
  • Blog »
  • i-sobot »
  • isobot with mbed -5 -array data for servos-

isobot with mbed -5 -array data for servos-

●Array data for servos
Last time we drove all servos by sending them the angle data. But it is not a good idea that you write angle data directly inside the sending part of the program when you create a complex action of the robot. So we use an array to hold angle data like

angle[key frame][joint] = {
{right_arm_1, right_arm_2, ...left_arm_1,left_arm_2,......left_leg_5},//start
{right_arm_1, right_arm_2, ...left_arm_1,left_arm_2,......left_leg_5},//after X seconds
{right_arm_1, right_arm_2, ...left_arm_1,left_arm_2,......left_leg_5},//after Y seconds
..........
}

It takes about 32msec to send angle data to all servos depending on the baud rate of uart. It is OK to have array of angle data for every 32 msec but it's too much. We use key frames of motion to hold distinguishing postures and calculate angles in between.
This time we only use stepwise key frame angles for every 30 frames(about 1sec).

#include "mbed.h"
#include "LPC11Uxx.h"

Ticker flipper;

DigitalOut dOut_0(P0_17);   //right arm
DigitalOut dOut_1(P0_20);   //left arm
DigitalOut dOut_2(P0_16);   //right leg
DigitalOut dOut_3(P0_2);    //left leg

//motion
int data[4][8];    //angle data for servos
int frameCount,keyFrameCount;
//softwareUART
unsigned char bitCount,byteCount;   //used in softwareUART

int angle[][20] =
    {
        //right arm             left arm                right leg               left leg
        {128,128,128,128,128,   128,128,128,128,128,    128,128,128,128,128,    128,128,128,128,128},   //keyFrameCount=0
        {148,128,128,128,128,   108,128,128,128,128,    128,128,128,128,128,    128,128,128,128,128},   //keyFrameCount=1
        {148,148,128,128,128,   108,108,128,128,128,    128,128,128,128,128,    128,128,128,128,128},   //keyFrameCount=2
        {148,148,148,128,128,   108,108,108,128,128,    128,128,128,128,128,    128,128,128,128,128}   //keyFrameCount=3
    };

void flip() {  //servo drive
    int sum;
    unsigned char out[4];
    
    //calclulate data at start of sending  (every 80*0.417msec)
    if(bitCount == 0 && byteCount == 0){
        for(int i=0; i<4; i++){ //right arm: i=0  left arm: i=1  right leg: i=2  left leg: i=3
            data[i][0] = 0xFF;    //header
            data[i][1] = 0x05;    //number of data
            data[i][2] = angle[keyFrameCount][i*5];     
            data[i][3] = angle[keyFrameCount][i*5+1];     
            data[i][4] = angle[keyFrameCount][i*5+2];
            data[i][5] = angle[keyFrameCount][i*5+3];     
            data[i][6] = angle[keyFrameCount][i*5+4];     
            //calc checksum
            data[i][7] = data[i][1] + data[i][2] + data[i][3] + data[i][4] + data[i][5] + data[i][6];
            sum = data[i][7];
            sum >>= 8;
            data[i][7] += sum;
        }
        frameCount++;
        if(frameCount > 30){    //30 frameCounts = 30*80*0.417msec = 1sec
            frameCount = 0;
            keyFrameCount++;    //update keyFrameCount(=line of angle[][])
            if(keyFrameCount > 3)keyFrameCount = 0;
        }
    }

    //send one bit data (software UART)
    if(bitCount == 0){
        out[0] = out[1] = out[2] = out[3] = 0;         //start bit
    }else if(bitCount == 9){
        out[0] = out[1] = out[2] = out[3] = 1;         //stop bit
    }else{
        out[0] = (data[0][byteCount] & (1 << (bitCount-1))) >> (bitCount-1);
        out[1] = (data[1][byteCount] & (1 << (bitCount-1))) >> (bitCount-1);
        out[2] = (data[2][byteCount] & (1 << (bitCount-1))) >> (bitCount-1);
        out[3] = (data[3][byteCount] & (1 << (bitCount-1))) >> (bitCount-1);
    }
    dOut_0 = out[0];
    dOut_1 = out[1];
    dOut_2 = out[2];
    dOut_3 = out[3];
    
    bitCount++;
    if(bitCount > 9){
        bitCount = 0;
        byteCount++;
        if(byteCount > 7)byteCount = 0;
    }
}

int main() {
    bitCount = byteCount = 0;
    frameCount = keyFrameCount = 0;
    //data[4] = 128;
    wait(2.0);
    flipper.attach_us(&flip, 417);
    while(1) { }
}

Please visit the shop.

Private: isobot with mbed -5 -array data for servos-

●Array data for servos
Last time we drove all servos by sending them the angle data. But it is not a good idea that you write angle data directly inside the sending part of the program when you create a complex action of the robot. So we use an array to hold angle data like

angle[key frame][joint] = {
{right_arm_1, right_arm_2, ...left_arm_1,left_arm_2,......left_leg_5},//start
{right_arm_1, right_arm_2, ...left_arm_1,left_arm_2,......left_leg_5},//after X seconds
{right_arm_1, right_arm_2, ...left_arm_1,left_arm_2,......left_leg_5},//after Y seconds
..........
}

It takes about 32msec to send angle data to all servos depending on the baud rate of uart. It is OK to have array of angle data for every 32 msec but it's too much. We use key frames of motion to hold distinguishing postures and calculate angles in between.
This time we only use stepwise key frame angles for every 30 frames(about 1sec).

#include "mbed.h"
#include "LPC11Uxx.h"

Ticker flipper;

DigitalOut dOut_0(P0_17);   //right arm
DigitalOut dOut_1(P0_20);   //left arm
DigitalOut dOut_2(P0_16);   //right leg
DigitalOut dOut_3(P0_2);    //left leg

//motion
int data[4][8];    //angle data for servos
int frameCount,keyFrameCount;
//softwareUART
unsigned char bitCount,byteCount;   //used in softwareUART

int angle[][20] =
    {
        //right arm             left arm                right leg               left leg
        {128,128,128,128,128,   128,128,128,128,128,    128,128,128,128,128,    128,128,128,128,128},   //keyFrameCount=0
        {148,128,128,128,128,   108,128,128,128,128,    128,128,128,128,128,    128,128,128,128,128},   //keyFrameCount=1
        {148,148,128,128,128,   108,108,128,128,128,    128,128,128,128,128,    128,128,128,128,128},   //keyFrameCount=2
        {148,148,148,128,128,   108,108,108,128,128,    128,128,128,128,128,    128,128,128,128,128}   //keyFrameCount=3
    };

void flip() {  //servo drive
    int sum;
    unsigned char out[4];
    
    //calclulate data at start of sending  (every 80*0.417msec)
    if(bitCount == 0 && byteCount == 0){
        for(int i=0; i<4; i++){ //right arm: i=0  left arm: i=1  right leg: i=2  left leg: i=3
            data[i][0] = 0xFF;    //header
            data[i][1] = 0x05;    //number of data
            data[i][2] = angle[keyFrameCount][i*5];     
            data[i][3] = angle[keyFrameCount][i*5+1];     
            data[i][4] = angle[keyFrameCount][i*5+2];
            data[i][5] = angle[keyFrameCount][i*5+3];     
            data[i][6] = angle[keyFrameCount][i*5+4];     
            //calc checksum
            data[i][7] = data[i][1] + data[i][2] + data[i][3] + data[i][4] + data[i][5] + data[i][6];
            sum = data[i][7];
            sum >>= 8;
            data[i][7] += sum;
        }
        frameCount++;
        if(frameCount > 30){    //30 frameCounts = 30*80*0.417msec = 1sec
            frameCount = 0;
            keyFrameCount++;    //update keyFrameCount(=line of angle[][])
            if(keyFrameCount > 3)keyFrameCount = 0;
        }
    }

    //send one bit data (software UART)
    if(bitCount == 0){
        out[0] = out[1] = out[2] = out[3] = 0;         //start bit
    }else if(bitCount == 9){
        out[0] = out[1] = out[2] = out[3] = 1;         //stop bit
    }else{
        out[0] = (data[0][byteCount] & (1 << (bitCount-1))) >> (bitCount-1);
        out[1] = (data[1][byteCount] & (1 << (bitCount-1))) >> (bitCount-1);
        out[2] = (data[2][byteCount] & (1 << (bitCount-1))) >> (bitCount-1);
        out[3] = (data[3][byteCount] & (1 << (bitCount-1))) >> (bitCount-1);
    }
    dOut_0 = out[0];
    dOut_1 = out[1];
    dOut_2 = out[2];
    dOut_3 = out[3];
    
    bitCount++;
    if(bitCount > 9){
        bitCount = 0;
        byteCount++;
        if(byteCount > 7)byteCount = 0;
    }
}

int main() {
    bitCount = byteCount = 0;
    frameCount = keyFrameCount = 0;
    //data[4] = 128;
    wait(2.0);
    flipper.attach_us(&flip, 417);
    while(1) { }
}

Please visit the shop.

contact


ゲームでプログラミング入門!


自作派のための本格指南書
好評発売中です

PAGETOP
Copyright © Meuse Robotics All Rights Reserved.
Powered by WordPress & BizVektor Theme by Vektor,Inc. technology.