Controlor for Humanoid. Walking trajectory generator, sensor reflection etc.
Dependencies: Adafruit-PWM-Servo-Driver MPU6050 RS300 mbed
Motions.cpp
- Committer:
- syundo0730
- Date:
- 2013-04-02
- Revision:
- 17:60de3bfdc70b
- Parent:
- 16:e65c192b7ecf
- Child:
- 18:7077bedc37eb
File content as of revision 17:60de3bfdc70b:
#include <iostream> #include <string> #include "Motions.h" Ticker tick; //detach may be better to controled from Motions class Motions::Motions(uint16_t* data) { pwm = new PWM(); //comu = new SCI(p28, p27); comu = new SCI(USBTX, USBRX); LocalFileSystem* local = new LocalFileSystem("local"); read("/local/motion.csv", data); set(data); playing = false; // Motion 0 is Home position setmotion(0); } 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); } 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 (!playing) { offline = new Motion(motions[id], pose_size[id], servo_size, pwm, &playing); tick.attach(offline, &Motion::step, TIMESTEP); //online = new OnlineMotion(1.0, TIMESTEP, servo_size, pwm, &playing); //tick.attach(online, &OnlineMotion::step, TIMESTEP); } } void Motions::control() { 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); }*/ }