![](/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
Diff: Motions.cpp
- Revision:
- 13:711f74b2fa33
- Parent:
- 12:6cd135bf03bd
- Child:
- 14:522bb06f0f0d
--- a/Motions.cpp Sun Feb 03 04:53:44 2013 +0000 +++ b/Motions.cpp Sun Feb 03 21:22:21 2013 +0000 @@ -9,12 +9,15 @@ 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); } @@ -28,13 +31,15 @@ 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; + 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) @@ -43,7 +48,7 @@ size_z = motion_size; size_x = servo_size; - uint16_t*** motions = new uint16_t**[size_z]; + motions = new uint16_t**[size_z]; uint16_t* p = data; for (int i = 0; i < size_z; ++i) { @@ -56,6 +61,15 @@ } } +bool Motions::checkid(int id) +{ + if (id >= 0 && id < motion_size) { + return true; + } else { + return false; + } +} + void Motions::play() { if (playing) { @@ -66,9 +80,10 @@ } } -void Motions::setmotion(const int& id) +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, 0.02); playing = true; @@ -77,10 +92,11 @@ void Motions::control() { - char id = 0; - id = comu->getid(); - if (id > 0) { + 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); } - play(); + } \ No newline at end of file