File content as of revision 23:0927e605af4b:

#include <iostream>
#include <string>

#include "Controlor.h"
#include "HomePosition.h"

const float TIMESTEP = 0.01;

//detach may be better to controled from Controlor class

Controlor::Controlor(uint16_t* data) : playing(false), attached(false), servo_size(10)
{
    //pwm = new PWM();
    pwm = new Adafruit_PWMServoDriver(p9, p10);
    pwm->begin();
    pwm->setPrescale(64);    //This value is decided for 10ms interval.
    pwm->setI2Cfreq(400000); //400kHz
    
    rs300 = new RS300(p28, p27);
    rs300->on_all_servo();
    
    home();
    
    //comu = new Console(p13, p14);
    comu = new Console(USBTX, USBRX);

    mode = 0;
    
    online = new OnlineMotion(0.7, TIMESTEP, servo_size, pwm, rs300, &playing);
    cpg = new CPG(TIMESTEP, pwm, &playing);
    playing = false;
}

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

void Controlor::home() {
	int pwm_servo_num = 10, serial_servo_num = 10;

	for (int i = 0; i < pwm_servo_num; ++i) {
		pwm->setDuty(i, HOMEPOS[i]);
	}
	//std::vector<uint16_t> buf(&HOMEPOS[pwm_servo_num], &HOMEPOS[pwm_servo_num + serial_servo_num]);
	std::vector<uint16_t> buf;
	for (int i = pwm_servo_num; i < pwm_servo_num + serial_servo_num; ++i) {
		buf.push_back(HOMEPOS[i]);
	}
	rs300->send_servo_pos(1, buf);
}

void Controlor::read(const string& filename, uint16_t* data)
{
    CSV csv;
    pose_size = new int;//<-This code is suspicious
    csv.read(filename, data, &servo_size, &motion_size, pose_size);
}

void Controlor::set(uint16_t* data)
{
    int size_z = motion_size;
    int 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 Controlor::checkid(int id)
{
    if (id >= 0 && id < motion_size) {
        return true;
    } else {
        return false;
    }
}

void Controlor::setmotion(const int id)
{
    // TODO : Make OfflineMotion class array and attach by each id. Newing every time is not good!
    //if (!motion.playing) {
        //motion = new OfflineMotion(motions[id], pose_size[id], servo_size, pwm, &playing);
        //tick.attach(motion, &Motion::step, TIMESTEP);
        //attached = true;
        //offline = new OfflineMotion(motions[id], pose_size[id], servo_size, pwm, &playing);
        //tick.attach(offline, &OfflineMotion::step, TIMESTEP);
        //online = new OnlineMotion(0.5, TIMESTEP, servo_size, pwm, &playing);
        //tick.attach(online, &OnlineMotion::step, TIMESTEP);
        //cpg = new CPG(TIMESTEP, pwm, &playing);
        //tick.attach(cpg, &CPG::step, TIMESTEP);
    //}
}

void Controlor::control()
{
    if (playing) {
        //online->step();
        cpg->step();
    }
    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->get_int_cr();
        pwm->setDuty(id, val);
    } else if (head == 'c') {
        cpg->change_param(0.1, 0.1, 0.1);
    } else if (head == 'd') {
        playing = true;
    } else if (head == 'e') {
    	playing = false;
    } else if (head == 'e') {
        comu->printf("Omega:\r\n");
        float omega = comu->get_float_cr();
        comu->printf("K:\r\n");
        float k = comu->get_float_cr();
        cpg->change_param(omega, k, 0);
    } else if (head == 'f') {
    	home();
    } else if (head == 'p') {
    	comu->showOnline(online);
    } 
}