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@12:6cd135bf03bd, 2013-02-03 (annotated)

Committer:
syundo0730
Date:
Sun Feb 03 04:53:44 2013 +0000
Revision:
12:6cd135bf03bd
Child:
13:711f74b2fa33
file reading, motion selecting, serial communication

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 12:6cd135bf03bd 12 Motions::Motions(uint16_t* data)
syundo0730 12:6cd135bf03bd 13 {
syundo0730 12:6cd135bf03bd 14 LocalFileSystem* local = new LocalFileSystem("local");
syundo0730 12:6cd135bf03bd 15 read("/local/motion.csv", data);
syundo0730 12:6cd135bf03bd 16 set(data);
syundo0730 12:6cd135bf03bd 17 playing = false;
syundo0730 12:6cd135bf03bd 18 comu = new SCI(USBTX, USBRX);
syundo0730 12:6cd135bf03bd 19 }
syundo0730 12:6cd135bf03bd 20
syundo0730 12:6cd135bf03bd 21 Motions::~Motions()
syundo0730 12:6cd135bf03bd 22 {
syundo0730 12:6cd135bf03bd 23 for (int i = 0; i < motion_size; i++) {
syundo0730 12:6cd135bf03bd 24 delete[] motions[i];
syundo0730 12:6cd135bf03bd 25 }
syundo0730 12:6cd135bf03bd 26 delete[] motions;
syundo0730 12:6cd135bf03bd 27 }
syundo0730 12:6cd135bf03bd 28
syundo0730 12:6cd135bf03bd 29 void Motions::read(const string& filename, uint16_t* data)
syundo0730 12:6cd135bf03bd 30 {
syundo0730 12:6cd135bf03bd 31 int srv, mtn;
syundo0730 12:6cd135bf03bd 32 int *pse = new int;
syundo0730 12:6cd135bf03bd 33 CSV csv;
syundo0730 12:6cd135bf03bd 34 csv.read(filename, data, &srv, &mtn, pse);
syundo0730 12:6cd135bf03bd 35 servo_size = srv;
syundo0730 12:6cd135bf03bd 36 motion_size = mtn;
syundo0730 12:6cd135bf03bd 37 pose_size = pse;
syundo0730 12:6cd135bf03bd 38 }
syundo0730 12:6cd135bf03bd 39
syundo0730 12:6cd135bf03bd 40 void Motions::set(uint16_t* data)
syundo0730 12:6cd135bf03bd 41 {
syundo0730 12:6cd135bf03bd 42 int size_z, size_x;
syundo0730 12:6cd135bf03bd 43 size_z = motion_size;
syundo0730 12:6cd135bf03bd 44 size_x = servo_size;
syundo0730 12:6cd135bf03bd 45
syundo0730 12:6cd135bf03bd 46 uint16_t*** motions = new uint16_t**[size_z];
syundo0730 12:6cd135bf03bd 47 uint16_t* p = data;
syundo0730 12:6cd135bf03bd 48
syundo0730 12:6cd135bf03bd 49 for (int i = 0; i < size_z; ++i) {
syundo0730 12:6cd135bf03bd 50 int size_y = pose_size[i];
syundo0730 12:6cd135bf03bd 51 motions[i] = new uint16_t*[size_y];
syundo0730 12:6cd135bf03bd 52 for (int j = 0; j < size_y; ++j) {
syundo0730 12:6cd135bf03bd 53 motions[i][j] = p + size_x * j;
syundo0730 12:6cd135bf03bd 54 }
syundo0730 12:6cd135bf03bd 55 p += size_x * size_y;
syundo0730 12:6cd135bf03bd 56 }
syundo0730 12:6cd135bf03bd 57 }
syundo0730 12:6cd135bf03bd 58
syundo0730 12:6cd135bf03bd 59 void Motions::play()
syundo0730 12:6cd135bf03bd 60 {
syundo0730 12:6cd135bf03bd 61 if (playing) {
syundo0730 12:6cd135bf03bd 62 if (!inter->is_in_interrupt()) {
syundo0730 12:6cd135bf03bd 63 delete inter;
syundo0730 12:6cd135bf03bd 64 playing = false;
syundo0730 12:6cd135bf03bd 65 }
syundo0730 12:6cd135bf03bd 66 }
syundo0730 12:6cd135bf03bd 67 }
syundo0730 12:6cd135bf03bd 68
syundo0730 12:6cd135bf03bd 69 void Motions::setmotion(const int& id)
syundo0730 12:6cd135bf03bd 70 {
syundo0730 12:6cd135bf03bd 71 if (!playing) {
syundo0730 12:6cd135bf03bd 72 inter = new Motion(motions[id], pose_size[id], servo_size);
syundo0730 12:6cd135bf03bd 73 tick.attach(inter, &Motion::step, 0.02);
syundo0730 12:6cd135bf03bd 74 playing = true;
syundo0730 12:6cd135bf03bd 75 }
syundo0730 12:6cd135bf03bd 76 }
syundo0730 12:6cd135bf03bd 77
syundo0730 12:6cd135bf03bd 78 void Motions::control()
syundo0730 12:6cd135bf03bd 79 {
syundo0730 12:6cd135bf03bd 80 char id = 0;
syundo0730 12:6cd135bf03bd 81 id = comu->getid();
syundo0730 12:6cd135bf03bd 82 if (id > 0) {
syundo0730 12:6cd135bf03bd 83 setmotion(id);
syundo0730 12:6cd135bf03bd 84 }
syundo0730 12:6cd135bf03bd 85 play();
syundo0730 12:6cd135bf03bd 86 }