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@17:60de3bfdc70b, 2013-04-02 (annotated)

Committer:
syundo0730
Date:
Tue Apr 02 04:19:09 2013 +0000
Revision:
17:60de3bfdc70b
Parent:
16:e65c192b7ecf
Child:
18:7077bedc37eb
+realtime module(not move well)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
syundo0730 12:6cd135bf03bd 1 #include <iostream>
syundo0730 12:6cd135bf03bd 2 #include <string>
syundo0730 17:60de3bfdc70b 3
syundo0730 12:6cd135bf03bd 4 #include "Motions.h"
syundo0730 12:6cd135bf03bd 5
syundo0730 17:60de3bfdc70b 6 Ticker tick;
syundo0730 12:6cd135bf03bd 7
syundo0730 17:60de3bfdc70b 8 //detach may be better to controled from Motions class
syundo0730 13:711f74b2fa33 9
syundo0730 12:6cd135bf03bd 10 Motions::Motions(uint16_t* data)
syundo0730 12:6cd135bf03bd 11 {
syundo0730 16:e65c192b7ecf 12 pwm = new PWM();
syundo0730 17:60de3bfdc70b 13 //comu = new SCI(p28, p27);
syundo0730 17:60de3bfdc70b 14
syundo0730 17:60de3bfdc70b 15 comu = new SCI(USBTX, USBRX);
syundo0730 17:60de3bfdc70b 16
syundo0730 12:6cd135bf03bd 17 LocalFileSystem* local = new LocalFileSystem("local");
syundo0730 17:60de3bfdc70b 18
syundo0730 12:6cd135bf03bd 19 read("/local/motion.csv", data);
syundo0730 12:6cd135bf03bd 20 set(data);
syundo0730 17:60de3bfdc70b 21
syundo0730 12:6cd135bf03bd 22 playing = false;
syundo0730 13:711f74b2fa33 23
syundo0730 17:60de3bfdc70b 24 // Motion 0 is Home position
syundo0730 17:60de3bfdc70b 25 setmotion(0);
syundo0730 12:6cd135bf03bd 26 }
syundo0730 12:6cd135bf03bd 27
syundo0730 12:6cd135bf03bd 28 Motions::~Motions()
syundo0730 12:6cd135bf03bd 29 {
syundo0730 12:6cd135bf03bd 30 for (int i = 0; i < motion_size; i++) {
syundo0730 12:6cd135bf03bd 31 delete[] motions[i];
syundo0730 12:6cd135bf03bd 32 }
syundo0730 12:6cd135bf03bd 33 delete[] motions;
syundo0730 16:e65c192b7ecf 34
syundo0730 16:e65c192b7ecf 35 delete comu;
syundo0730 16:e65c192b7ecf 36 delete pwm;
syundo0730 12:6cd135bf03bd 37 }
syundo0730 12:6cd135bf03bd 38
syundo0730 12:6cd135bf03bd 39 void Motions::read(const string& filename, uint16_t* data)
syundo0730 12:6cd135bf03bd 40 {
syundo0730 12:6cd135bf03bd 41 CSV csv;
syundo0730 13:711f74b2fa33 42 pose_size = new int;
syundo0730 13:711f74b2fa33 43 csv.read(filename, data, &servo_size, &motion_size, pose_size);
syundo0730 12:6cd135bf03bd 44 }
syundo0730 12:6cd135bf03bd 45
syundo0730 12:6cd135bf03bd 46 void Motions::set(uint16_t* data)
syundo0730 12:6cd135bf03bd 47 {
syundo0730 12:6cd135bf03bd 48 int size_z, size_x;
syundo0730 12:6cd135bf03bd 49 size_z = motion_size;
syundo0730 12:6cd135bf03bd 50 size_x = servo_size;
syundo0730 12:6cd135bf03bd 51
syundo0730 13:711f74b2fa33 52 motions = new uint16_t**[size_z];
syundo0730 12:6cd135bf03bd 53 uint16_t* p = data;
syundo0730 12:6cd135bf03bd 54
syundo0730 12:6cd135bf03bd 55 for (int i = 0; i < size_z; ++i) {
syundo0730 12:6cd135bf03bd 56 int size_y = pose_size[i];
syundo0730 12:6cd135bf03bd 57 motions[i] = new uint16_t*[size_y];
syundo0730 12:6cd135bf03bd 58 for (int j = 0; j < size_y; ++j) {
syundo0730 12:6cd135bf03bd 59 motions[i][j] = p + size_x * j;
syundo0730 12:6cd135bf03bd 60 }
syundo0730 12:6cd135bf03bd 61 p += size_x * size_y;
syundo0730 12:6cd135bf03bd 62 }
syundo0730 12:6cd135bf03bd 63 }
syundo0730 12:6cd135bf03bd 64
syundo0730 13:711f74b2fa33 65 bool Motions::checkid(int id)
syundo0730 13:711f74b2fa33 66 {
syundo0730 13:711f74b2fa33 67 if (id >= 0 && id < motion_size) {
syundo0730 13:711f74b2fa33 68 return true;
syundo0730 13:711f74b2fa33 69 } else {
syundo0730 13:711f74b2fa33 70 return false;
syundo0730 13:711f74b2fa33 71 }
syundo0730 13:711f74b2fa33 72 }
syundo0730 13:711f74b2fa33 73
syundo0730 15:e37a8c413e51 74 void Motions::setmotion(const int id)
syundo0730 14:522bb06f0f0d 75 {
syundo0730 17:60de3bfdc70b 76 if (!playing) {
syundo0730 17:60de3bfdc70b 77 offline = new Motion(motions[id], pose_size[id], servo_size, pwm, &playing);
syundo0730 17:60de3bfdc70b 78 tick.attach(offline, &Motion::step, TIMESTEP);
syundo0730 17:60de3bfdc70b 79 //online = new OnlineMotion(1.0, TIMESTEP, servo_size, pwm, &playing);
syundo0730 17:60de3bfdc70b 80 //tick.attach(online, &OnlineMotion::step, TIMESTEP);
syundo0730 15:e37a8c413e51 81 }
syundo0730 15:e37a8c413e51 82 }
syundo0730 14:522bb06f0f0d 83
syundo0730 12:6cd135bf03bd 84 void Motions::control()
syundo0730 12:6cd135bf03bd 85 {
syundo0730 17:60de3bfdc70b 86 setmotion(1);
syundo0730 17:60de3bfdc70b 87 /*char head = comu->getheader();
syundo0730 16:e65c192b7ecf 88 if (head == 'A') {
syundo0730 16:e65c192b7ecf 89 int id = comu->getid();
syundo0730 16:e65c192b7ecf 90 if (checkid(id)) {
syundo0730 16:e65c192b7ecf 91 setmotion(id);
syundo0730 16:e65c192b7ecf 92 }
syundo0730 16:e65c192b7ecf 93 } else if (head == 'B') {
syundo0730 16:e65c192b7ecf 94 int id = comu->getid();
syundo0730 16:e65c192b7ecf 95 uint16_t val = comu->getservoval();
syundo0730 16:e65c192b7ecf 96 pwm->SetDuty(id, (uint32_t)val);
syundo0730 17:60de3bfdc70b 97 }*/
syundo0730 13:711f74b2fa33 98
syundo0730 12:6cd135bf03bd 99 }