File content as of revision 16:e65c192b7ecf:

#include <iostream>
#include <string>
#include "mbed.h"
#include "CSV.h"
#include "Motions.h"
#include "Motion.h"
//#include "SCI.h"
//#include "PWM.h"

extern uint16_t data[0x2000] __attribute__((section("AHBSRAM1")));
extern Ticker tick;

bool g_motion_playing = false;

Serial serial(USBTX, USBRX);
//Serial device(p28, p27);

Motions::Motions(uint16_t* data)
{
    pwm = new PWM();
    LocalFileSystem* local = new LocalFileSystem("local");
    read("/local/motion.csv", data);
    set(data);
    playing = false;
    
    //comu = new SCI(USBTX, USBRX);
    comu = new SCI(p28, p27);
}

Motions::~Motions()
{
    for (int i = 0; i < motion_size; i++) {
        delete[] motions[i];
    }
    delete[] motions;
    
    delete comu;
    delete pwm;
}

void Motions::read(const string& filename, uint16_t* data)
{
    CSV csv;
    pose_size = new int;
    csv.read(filename, data, &servo_size, &motion_size, pose_size);
    
    //serial.printf("readed!\r\n");
    //serial.printf("servo_size:%d motion_size:%d\r\n", servo_size, motion_size);
    //for (int i = 0; i < motion_size; ++i) {
        //serial.printf("motion %d pose_size:%d\r\n", i, pose_size[i]);
    //}
}

void Motions::set(uint16_t* data)
{
    int size_z, size_x;
    size_z = motion_size;
    size_x = servo_size;
    
    motions = new uint16_t**[size_z];
    uint16_t* p = data;
    
    for (int i = 0; i < size_z; ++i) {
        int size_y = pose_size[i];
        motions[i] = new uint16_t*[size_y];
        for (int j = 0; j < size_y; ++j) {
            motions[i][j] = p + size_x * j;
        }
        p += size_x * size_y;
    }
}

bool Motions::checkid(int id)
{
    if (id >= 0 && id < motion_size) {
        return true;
    } else {
        return false;
    }
}

void Motions::setmotion(const int id)
{
    if (!g_motion_playing) {
        inter = new Motion(motions[id], pose_size[id], servo_size, pwm);
        tick.attach(inter, &Motion::step, TIMESTEP);
        playing = true;
    }
}

void Motions::control()
{
    char head = comu->getheader();
    if (head == 'A') {
        int id = comu->getid();
        if (checkid(id)) {
            setmotion(id);
        }
    } else if (head == 'B') {
        int id = comu->getid();
        uint16_t val = comu->getservoval();
        pwm->SetDuty(id, (uint32_t)val);
    }
    
}