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@15:e37a8c413e51, 2013-02-21 (annotated)

Committer:
syundo0730
Date:
Thu Feb 21 15:21:12 2013 +0000
Revision:
15:e37a8c413e51
Parent:
14:522bb06f0f0d
Child:
16:e65c192b7ecf
No need for watching whether in interrupt or not. (Global value is used. Need modification )

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