![](/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
Motions.cpp
- Committer:
- syundo0730
- Date:
- 2013-02-21
- Revision:
- 14:522bb06f0f0d
- Parent:
- 13:711f74b2fa33
- Child:
- 15:e37a8c413e51
File content as of revision 14:522bb06f0f0d:
#include <iostream> #include <string> #include "mbed.h" #include "CSV.h" #include "Motions.h" #include "Motion.h" #include "SCI.h" extern uint16_t data[0x2000] __attribute__((section("AHBSRAM1"))); extern Ticker tick; //Serial serial(USBTX, USBRX); Motions::Motions(uint16_t* data) { LocalFileSystem* local = new LocalFileSystem("local"); read("/local/motion.csv", data); set(data); playing = false; comu = new SCI(USBTX, USBRX); } Motions::~Motions() { for (int i = 0; i < motion_size; i++) { delete[] motions[i]; } delete[] motions; } 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::play() { if (playing) { if (!inter->is_in_interrupt()) { delete inter; playing = false; } } } void Motions::setmotion(const int id) { if (!playing) { //serial.printf("start motion! id = %d \r\n", id); inter = new Motion(motions[id], pose_size[id], servo_size); tick.attach(inter, &Motion::step, TIMESTEP); playing = true; } } /*void Motions::setmotion(const int id) { //if (inter == NULL) { //serial.printf("start motion! id = %d \r\n", id); inter = new Motion(motions[id], pose_size[id], servo_size); tick.attach(inter, &Motion::step, TIMESTEP); playing = true; //} }*/ void Motions::control() { play(); //int id = comu->getid(); //serial.printf("id: %d \r\n", id); //if (checkid(id)) { //<- Misterious bug. Why is id starts from 48? //setmotion(id); setmotion(0); //} }