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

Controlor.cpp@20:abb7852df747, 2013-08-19 (annotated)

Committer:
syundo0730
Date:
Mon Aug 19 08:10:58 2013 +0000
Revision:
20:abb7852df747
Parent:
Motions.cpp@19:c2ec475367aa
Child:
21:a54bcab078ed
delete Motion instance from Control class

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 20:abb7852df747 4 #include "Controlor.h"
syundo0730 12:6cd135bf03bd 5
syundo0730 17:60de3bfdc70b 6 Ticker tick;
syundo0730 12:6cd135bf03bd 7
syundo0730 20:abb7852df747 8 //detach may be better to controled from Controlor class
syundo0730 13:711f74b2fa33 9
syundo0730 20:abb7852df747 10 Controlor::Controlor(uint16_t* data) : playing(false), attached(false)
syundo0730 12:6cd135bf03bd 11 {
syundo0730 16:e65c192b7ecf 12 pwm = new PWM();
syundo0730 17:60de3bfdc70b 13 //comu = new SCI(p28, p27);
syundo0730 17:60de3bfdc70b 14 comu = new SCI(USBTX, USBRX);
syundo0730 17:60de3bfdc70b 15
syundo0730 12:6cd135bf03bd 16 LocalFileSystem* local = new LocalFileSystem("local");
syundo0730 17:60de3bfdc70b 17
syundo0730 12:6cd135bf03bd 18 read("/local/motion.csv", data);
syundo0730 12:6cd135bf03bd 19 set(data);
syundo0730 17:60de3bfdc70b 20
syundo0730 17:60de3bfdc70b 21 // Motion 0 is Home position
syundo0730 17:60de3bfdc70b 22 setmotion(0);
syundo0730 12:6cd135bf03bd 23 }
syundo0730 12:6cd135bf03bd 24
syundo0730 20:abb7852df747 25 Controlor::~Controlor()
syundo0730 12:6cd135bf03bd 26 {
syundo0730 12:6cd135bf03bd 27 for (int i = 0; i < motion_size; i++) {
syundo0730 12:6cd135bf03bd 28 delete[] motions[i];
syundo0730 12:6cd135bf03bd 29 }
syundo0730 12:6cd135bf03bd 30 delete[] motions;
syundo0730 16:e65c192b7ecf 31
syundo0730 16:e65c192b7ecf 32 delete comu;
syundo0730 16:e65c192b7ecf 33 delete pwm;
syundo0730 12:6cd135bf03bd 34 }
syundo0730 12:6cd135bf03bd 35
syundo0730 20:abb7852df747 36 void Controlor::read(const string& filename, uint16_t* data)
syundo0730 12:6cd135bf03bd 37 {
syundo0730 12:6cd135bf03bd 38 CSV csv;
syundo0730 20:abb7852df747 39 pose_size = new int;//<-This code is suspicious
syundo0730 13:711f74b2fa33 40 csv.read(filename, data, &servo_size, &motion_size, pose_size);
syundo0730 19:c2ec475367aa 41 //readMotion(filename, data, servo_size, motion_size, pose_size); // not so good at speed and has bug in handling float motion value.
syundo0730 12:6cd135bf03bd 42 }
syundo0730 12:6cd135bf03bd 43
syundo0730 20:abb7852df747 44 void Controlor::set(uint16_t* data)
syundo0730 12:6cd135bf03bd 45 {
syundo0730 20:abb7852df747 46 int size_z = motion_size;
syundo0730 20:abb7852df747 47 int size_x = servo_size;
syundo0730 12:6cd135bf03bd 48
syundo0730 13:711f74b2fa33 49 motions = new uint16_t**[size_z];
syundo0730 12:6cd135bf03bd 50 uint16_t* p = data;
syundo0730 12:6cd135bf03bd 51
syundo0730 12:6cd135bf03bd 52 for (int i = 0; i < size_z; ++i) {
syundo0730 12:6cd135bf03bd 53 int size_y = pose_size[i];
syundo0730 12:6cd135bf03bd 54 motions[i] = new uint16_t*[size_y];
syundo0730 12:6cd135bf03bd 55 for (int j = 0; j < size_y; ++j) {
syundo0730 12:6cd135bf03bd 56 motions[i][j] = p + size_x * j;
syundo0730 12:6cd135bf03bd 57 }
syundo0730 12:6cd135bf03bd 58 p += size_x * size_y;
syundo0730 12:6cd135bf03bd 59 }
syundo0730 12:6cd135bf03bd 60 }
syundo0730 12:6cd135bf03bd 61
syundo0730 20:abb7852df747 62 bool Controlor::checkid(int id)
syundo0730 13:711f74b2fa33 63 {
syundo0730 13:711f74b2fa33 64 if (id >= 0 && id < motion_size) {
syundo0730 13:711f74b2fa33 65 return true;
syundo0730 13:711f74b2fa33 66 } else {
syundo0730 13:711f74b2fa33 67 return false;
syundo0730 13:711f74b2fa33 68 }
syundo0730 13:711f74b2fa33 69 }
syundo0730 13:711f74b2fa33 70
syundo0730 20:abb7852df747 71 void Controlor::setmotion(const int id)
syundo0730 14:522bb06f0f0d 72 {
syundo0730 20:abb7852df747 73 // TODO : Make OfflineMotion class array and attach by each id. Newing every time is not good!
syundo0730 20:abb7852df747 74 //if (!motion.playing) {
syundo0730 20:abb7852df747 75 //motion = new OfflineMotion(motions[id], pose_size[id], servo_size, pwm, &playing);
syundo0730 20:abb7852df747 76 //tick.attach(motion, &Motion::step, TIMESTEP);
syundo0730 20:abb7852df747 77 attached = true;
syundo0730 20:abb7852df747 78 offline = new OfflineMotion(motions[id], pose_size[id], servo_size, pwm, &playing);
syundo0730 20:abb7852df747 79 tick.attach(offline, &OfflineMotion::step, TIMESTEP);
syundo0730 19:c2ec475367aa 80 //online = new OnlineMotion(3.0, TIMESTEP, servo_size, pwm, &playing);
syundo0730 19:c2ec475367aa 81 //tick.attach(online, &OnlineMotion::step, TIMESTEP);
syundo0730 20:abb7852df747 82 //}
syundo0730 15:e37a8c413e51 83 }
syundo0730 14:522bb06f0f0d 84
syundo0730 20:abb7852df747 85 void Controlor::control()
syundo0730 12:6cd135bf03bd 86 {
syundo0730 20:abb7852df747 87 if (!playing) {
syundo0730 20:abb7852df747 88 if (attached) {
syundo0730 20:abb7852df747 89 tick.detach();
syundo0730 20:abb7852df747 90 delete offline;
syundo0730 20:abb7852df747 91 attached = false;
syundo0730 20:abb7852df747 92 } else {
syundo0730 20:abb7852df747 93 setmotion(1);
syundo0730 20:abb7852df747 94 }
syundo0730 20:abb7852df747 95 }
syundo0730 20:abb7852df747 96
syundo0730 20:abb7852df747 97 //setmotion(1);
syundo0730 17:60de3bfdc70b 98 /*char head = comu->getheader();
syundo0730 16:e65c192b7ecf 99 if (head == 'A') {
syundo0730 16:e65c192b7ecf 100 int id = comu->getid();
syundo0730 16:e65c192b7ecf 101 if (checkid(id)) {
syundo0730 16:e65c192b7ecf 102 setmotion(id);
syundo0730 16:e65c192b7ecf 103 }
syundo0730 16:e65c192b7ecf 104 } else if (head == 'B') {
syundo0730 16:e65c192b7ecf 105 int id = comu->getid();
syundo0730 16:e65c192b7ecf 106 uint16_t val = comu->getservoval();
syundo0730 16:e65c192b7ecf 107 pwm->SetDuty(id, (uint32_t)val);
syundo0730 17:60de3bfdc70b 108 }*/
syundo0730 13:711f74b2fa33 109
syundo0730 12:6cd135bf03bd 110 }