Shundo Kishi / Mbed 2 deprecated Hobby_Humanoid_controlor

Controlor for Humanoid. Walking trajectory generator, sensor reflection etc.

Dependencies:   Adafruit-PWM-Servo-Driver MPU6050 RS300 mbed

Motions.cpp@13:711f74b2fa33, 2013-02-03 (annotated)

Committer:
syundo0730
Date:
Sun Feb 03 21:22:21 2013 +0000
Revision:
13:711f74b2fa33
Parent:
12:6cd135bf03bd
Child:
14:522bb06f0f0d
play motion by signal from serial port

Who changed what in which revision?

UserRevisionLine numberNew contents of line
syundo0730 12:6cd135bf03bd 1 #include <iostream>
syundo0730 12:6cd135bf03bd 2 #include <string>
syundo0730 12:6cd135bf03bd 3 #include "mbed.h"
syundo0730 12:6cd135bf03bd 4 #include "CSV.h"
syundo0730 12:6cd135bf03bd 5 #include "Motions.h"
syundo0730 12:6cd135bf03bd 6 #include "Motion.h"
syundo0730 12:6cd135bf03bd 7 #include "SCI.h"
syundo0730 12:6cd135bf03bd 8
syundo0730 12:6cd135bf03bd 9 extern uint16_t data[0x2000] __attribute__((section("AHBSRAM1")));
syundo0730 12:6cd135bf03bd 10 extern Ticker tick;
syundo0730 12:6cd135bf03bd 11
syundo0730 13:711f74b2fa33 12 //Serial serial(USBTX, USBRX);
syundo0730 13:711f74b2fa33 13
syundo0730 12:6cd135bf03bd 14 Motions::Motions(uint16_t* data)
syundo0730 12:6cd135bf03bd 15 {
syundo0730 12:6cd135bf03bd 16 LocalFileSystem* local = new LocalFileSystem("local");
syundo0730 12:6cd135bf03bd 17 read("/local/motion.csv", data);
syundo0730 12:6cd135bf03bd 18 set(data);
syundo0730 12:6cd135bf03bd 19 playing = false;
syundo0730 13:711f74b2fa33 20
syundo0730 12:6cd135bf03bd 21 comu = new SCI(USBTX, USBRX);
syundo0730 12:6cd135bf03bd 22 }
syundo0730 12:6cd135bf03bd 23
syundo0730 12:6cd135bf03bd 24 Motions::~Motions()
syundo0730 12:6cd135bf03bd 25 {
syundo0730 12:6cd135bf03bd 26 for (int i = 0; i < motion_size; i++) {
syundo0730 12:6cd135bf03bd 27 delete[] motions[i];
syundo0730 12:6cd135bf03bd 28 }
syundo0730 12:6cd135bf03bd 29 delete[] motions;
syundo0730 12:6cd135bf03bd 30 }
syundo0730 12:6cd135bf03bd 31
syundo0730 12:6cd135bf03bd 32 void Motions::read(const string& filename, uint16_t* data)
syundo0730 12:6cd135bf03bd 33 {
syundo0730 12:6cd135bf03bd 34 CSV csv;
syundo0730 13:711f74b2fa33 35 pose_size = new int;
syundo0730 13:711f74b2fa33 36 csv.read(filename, data, &servo_size, &motion_size, pose_size);
syundo0730 13:711f74b2fa33 37
syundo0730 13:711f74b2fa33 38 //serial.printf("readed!\r\n");
syundo0730 13:711f74b2fa33 39 //serial.printf("servo_size:%d motion_size:%d\r\n", servo_size, motion_size);
syundo0730 13:711f74b2fa33 40 //for (int i = 0; i < motion_size; ++i) {
syundo0730 13:711f74b2fa33 41 //serial.printf("motion %d pose_size:%d\r\n", i, pose_size[i]);
syundo0730 13:711f74b2fa33 42 //}
syundo0730 12:6cd135bf03bd 43 }
syundo0730 12:6cd135bf03bd 44
syundo0730 12:6cd135bf03bd 45 void Motions::set(uint16_t* data)
syundo0730 12:6cd135bf03bd 46 {
syundo0730 12:6cd135bf03bd 47 int size_z, size_x;
syundo0730 12:6cd135bf03bd 48 size_z = motion_size;
syundo0730 12:6cd135bf03bd 49 size_x = servo_size;
syundo0730 12:6cd135bf03bd 50
syundo0730 13:711f74b2fa33 51 motions = new uint16_t**[size_z];
syundo0730 12:6cd135bf03bd 52 uint16_t* p = data;
syundo0730 12:6cd135bf03bd 53
syundo0730 12:6cd135bf03bd 54 for (int i = 0; i < size_z; ++i) {
syundo0730 12:6cd135bf03bd 55 int size_y = pose_size[i];
syundo0730 12:6cd135bf03bd 56 motions[i] = new uint16_t*[size_y];
syundo0730 12:6cd135bf03bd 57 for (int j = 0; j < size_y; ++j) {
syundo0730 12:6cd135bf03bd 58 motions[i][j] = p + size_x * j;
syundo0730 12:6cd135bf03bd 59 }
syundo0730 12:6cd135bf03bd 60 p += size_x * size_y;
syundo0730 12:6cd135bf03bd 61 }
syundo0730 12:6cd135bf03bd 62 }
syundo0730 12:6cd135bf03bd 63
syundo0730 13:711f74b2fa33 64 bool Motions::checkid(int id)
syundo0730 13:711f74b2fa33 65 {
syundo0730 13:711f74b2fa33 66 if (id >= 0 && id < motion_size) {
syundo0730 13:711f74b2fa33 67 return true;
syundo0730 13:711f74b2fa33 68 } else {
syundo0730 13:711f74b2fa33 69 return false;
syundo0730 13:711f74b2fa33 70 }
syundo0730 13:711f74b2fa33 71 }
syundo0730 13:711f74b2fa33 72
syundo0730 12:6cd135bf03bd 73 void Motions::play()
syundo0730 12:6cd135bf03bd 74 {
syundo0730 12:6cd135bf03bd 75 if (playing) {
syundo0730 12:6cd135bf03bd 76 if (!inter->is_in_interrupt()) {
syundo0730 12:6cd135bf03bd 77 delete inter;
syundo0730 12:6cd135bf03bd 78 playing = false;
syundo0730 12:6cd135bf03bd 79 }
syundo0730 12:6cd135bf03bd 80 }
syundo0730 12:6cd135bf03bd 81 }
syundo0730 12:6cd135bf03bd 82
syundo0730 13:711f74b2fa33 83 void Motions::setmotion(const int id)
syundo0730 12:6cd135bf03bd 84 {
syundo0730 12:6cd135bf03bd 85 if (!playing) {
syundo0730 13:711f74b2fa33 86 //serial.printf("start motion! id = %d \r\n", id);
syundo0730 12:6cd135bf03bd 87 inter = new Motion(motions[id], pose_size[id], servo_size);
syundo0730 12:6cd135bf03bd 88 tick.attach(inter, &Motion::step, 0.02);
syundo0730 12:6cd135bf03bd 89 playing = true;
syundo0730 12:6cd135bf03bd 90 }
syundo0730 12:6cd135bf03bd 91 }
syundo0730 12:6cd135bf03bd 92
syundo0730 12:6cd135bf03bd 93 void Motions::control()
syundo0730 12:6cd135bf03bd 94 {
syundo0730 13:711f74b2fa33 95 play();
syundo0730 13:711f74b2fa33 96 int id = comu->getid();
syundo0730 13:711f74b2fa33 97 //serial.printf("id: %d \r\n", id);
syundo0730 13:711f74b2fa33 98 if (checkid(id)) { //<- Misterious bug. Why is id starts from 48?
syundo0730 12:6cd135bf03bd 99 setmotion(id);
syundo0730 12:6cd135bf03bd 100 }
syundo0730 13:711f74b2fa33 101
syundo0730 12:6cd135bf03bd 102 }