![](/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-03
- Revision:
- 12:6cd135bf03bd
- Child:
- 13:711f74b2fa33
File content as of revision 12:6cd135bf03bd:
#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; 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) { int srv, mtn; int *pse = new int; CSV csv; csv.read(filename, data, &srv, &mtn, pse); servo_size = srv; motion_size = mtn; pose_size = pse; } void Motions::set(uint16_t* data) { int size_z, size_x; size_z = motion_size; size_x = servo_size; uint16_t*** 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; } } void Motions::play() { if (playing) { if (!inter->is_in_interrupt()) { delete inter; playing = false; } } } void Motions::setmotion(const int& id) { if (!playing) { inter = new Motion(motions[id], pose_size[id], servo_size); tick.attach(inter, &Motion::step, 0.02); playing = true; } } void Motions::control() { char id = 0; id = comu->getid(); if (id > 0) { setmotion(id); } play(); }