isobot with mbed -6- interpolate between key frames

●interpolate between key frames
This time we change the way of getting target angles to make joints to move smoothly not like steps.
At time x between two key frames A and B, target angle are given by
angX = angA + (angB - angA) * tX / (tB - tA)
In the program A and B are called keyFrame and nextKeyFrame.

#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;
int nextKeyFrameCount;
//softwareUART
unsigned char bitCount,byteCount;   //used in softwareUART

int angle[][20] =
    {
        {128,128,128,128,128,   128,128,128,128,128,    128,128,128,128,128,    128,128,128,128,128},
        {148,128,128,128,128,   108,128,128,128,128,    128,128,128,128,128,    128,128,128,128,128},
        {148,148,128,128,128,   108,108,128,128,128,    128,128,128,128,128,    128,128,128,128,128},
        {148,148,148,128,128,   108,108,108,128,128,    128,128,128,128,128,    128,128,128,128,128}
    };

void flip() {  //servo drive
    int sum;
    unsigned char out[4];
    int divNum = 30;
    //calclulate data at start of sending  (every 80*0.417msec)
    if(bitCount == 0 && byteCount == 0){
        for(int i=0; i<4; i++){
            data[i][0] = 0xFF;    //header
            data[i][1] = 0x05;    //number of data
            data[i][2] = angle[keyFrameCount][i*5] 
                            + (angle[nextKeyFrameCount][i*5] - angle[keyFrameCount][i*5])
                            * frameCount / divNum;
            data[i][3] = angle[keyFrameCount][i*5+1] 
                            + (angle[nextKeyFrameCount][i*5+1] - angle[keyFrameCount][i*5+1])
                            * frameCount / divNum;
            data[i][4] = angle[keyFrameCount][i*5+2] 
                            + (angle[nextKeyFrameCount][i*5+2] - angle[keyFrameCount][i*5+2])
                            * frameCount / divNum;
            data[i][5] = angle[keyFrameCount][i*5+3] 
                            + (angle[nextKeyFrameCount][i*5+3] - angle[keyFrameCount][i*5+3])
                            * frameCount / divNum;
            data[i][6] = angle[keyFrameCount][i*5+4] 
                            + (angle[nextKeyFrameCount][i*5+4] - angle[keyFrameCount][i*5+4])
                            * frameCount / divNum;
            //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 > divNum){
            frameCount = 0;
            keyFrameCount++;
            if(keyFrameCount > 3)keyFrameCount = 0;
            nextKeyFrameCount++;
            if(nextKeyFrameCount > 3)nextKeyFrameCount = 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;
    nextKeyFrameCount = 1;
    //data[4] = 128;
    wait(2.0);
    flipper.attach_us(&flip, 417);
    while(1) { }
}

Please visit the shop.