![](/media/cache/profiles/e5815151957be36ad2085b7a1a02c5cc.jpg.50x50_q85.jpg)
Controlor for Humanoid. Walking trajectory generator, sensor reflection etc.
Dependencies: Adafruit-PWM-Servo-Driver MPU6050 RS300 mbed
Controlor.cpp
- Committer:
- syundo0730
- Date:
- 2013-09-04
- Revision:
- 21:a54bcab078ed
- Parent:
- 20:abb7852df747
- Child:
- 22:bf5aa20b9df0
File content as of revision 21:a54bcab078ed:
#include <iostream> #include <string> #include "Controlor.h" Ticker tick; const float TIMESTEP = 0.01; //detach may be better to controled from Controlor class Controlor::Controlor(uint16_t* data) : playing(false), attached(false) { //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 //comu = new SCI(p28, p27); comu = new SCI(USBTX, USBRX); LocalFileSystem* local = new LocalFileSystem("local"); read("/local/motion.csv", data); set(data); // Motion 0 is Home position setmotion(0); } Controlor::~Controlor() { for (int i = 0; i < motion_size; i++) { delete[] motions[i]; } delete[] motions; delete comu; delete pwm; } 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); //readMotion(filename, data, servo_size, motion_size, pose_size); // not so good at speed and has bug in handling float motion value. } 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(1.0, TIMESTEP, servo_size, pwm, &playing); //tick.attach(online, &OnlineMotion::step, TIMESTEP); //} } void Controlor::control() { if (!playing) { if (attached) { tick.detach(); delete offline; attached = false; } else { setmotion(1); } } //setmotion(1); /*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); }*/ }