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@22:bf5aa20b9df0, 2013-09-06 (annotated)

Committer:
syundo0730
Date:
Fri Sep 06 08:36:21 2013 +0000
Revision:
22:bf5aa20b9df0
Parent:
21:a54bcab078ed
Child:
23:0927e605af4b
Subtile change.; Home position was changed.; SCI class was renamed to Console class and function was modified.

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