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@23:0927e605af4b, 2013-11-22 (annotated)

Committer:
syundo0730
Date:
Fri Nov 22 00:30:42 2013 +0000
Revision:
23:0927e605af4b
Parent:
22:bf5aa20b9df0 
first commit

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 23:0927e605af4b 7 const float TIMESTEP = 0.01;
syundo0730 21:a54bcab078ed 8
syundo0730 20:abb7852df747 9 //detach may be better to controled from Controlor class
syundo0730 13:711f74b2fa33 10
syundo0730 23:0927e605af4b 11 Controlor::Controlor(uint16_t* data) : playing(false), attached(false), servo_size(10)
syundo0730 12:6cd135bf03bd 12 {
syundo0730 21:a54bcab078ed 13 //pwm = new PWM();
syundo0730 21:a54bcab078ed 14 pwm = new Adafruit_PWMServoDriver(p9, p10);
syundo0730 21:a54bcab078ed 15 pwm->begin();
syundo0730 21:a54bcab078ed 16 pwm->setPrescale(64); //This value is decided for 10ms interval.
syundo0730 21:a54bcab078ed 17 pwm->setI2Cfreq(400000); //400kHz
syundo0730 21:a54bcab078ed 18
syundo0730 23:0927e605af4b 19 rs300 = new RS300(p28, p27);
syundo0730 23:0927e605af4b 20 rs300->on_all_servo();
syundo0730 22:bf5aa20b9df0 21
syundo0730 23:0927e605af4b 22 home();
syundo0730 17:60de3bfdc70b 23
syundo0730 23:0927e605af4b 24 //comu = new Console(p13, p14);
syundo0730 23:0927e605af4b 25 comu = new Console(USBTX, USBRX);
syundo0730 23:0927e605af4b 26
syundo0730 23:0927e605af4b 27 mode = 0;
syundo0730 17:60de3bfdc70b 28
syundo0730 23:0927e605af4b 29 online = new OnlineMotion(0.7, TIMESTEP, servo_size, pwm, rs300, &playing);
syundo0730 23:0927e605af4b 30 cpg = new CPG(TIMESTEP, pwm, &playing);
syundo0730 23:0927e605af4b 31 playing = false;
syundo0730 12:6cd135bf03bd 32 }
syundo0730 12:6cd135bf03bd 33
syundo0730 20:abb7852df747 34 Controlor::~Controlor()
syundo0730 12:6cd135bf03bd 35 {
syundo0730 12:6cd135bf03bd 36 for (int i = 0; i < motion_size; i++) {
syundo0730 12:6cd135bf03bd 37 delete[] motions[i];
syundo0730 12:6cd135bf03bd 38 }
syundo0730 12:6cd135bf03bd 39 delete[] motions;
syundo0730 16:e65c192b7ecf 40
syundo0730 16:e65c192b7ecf 41 delete comu;
syundo0730 16:e65c192b7ecf 42 delete pwm;
syundo0730 12:6cd135bf03bd 43 }
syundo0730 12:6cd135bf03bd 44
syundo0730 23:0927e605af4b 45 void Controlor::home() {
syundo0730 23:0927e605af4b 46 int pwm_servo_num = 10, serial_servo_num = 10;
syundo0730 23:0927e605af4b 47
syundo0730 23:0927e605af4b 48 for (int i = 0; i < pwm_servo_num; ++i) {
syundo0730 23:0927e605af4b 49 pwm->setDuty(i, HOMEPOS[i]);
syundo0730 23:0927e605af4b 50 }
syundo0730 23:0927e605af4b 51 //std::vector<uint16_t> buf(&HOMEPOS[pwm_servo_num], &HOMEPOS[pwm_servo_num + serial_servo_num]);
syundo0730 23:0927e605af4b 52 std::vector<uint16_t> buf;
syundo0730 23:0927e605af4b 53 for (int i = pwm_servo_num; i < pwm_servo_num + serial_servo_num; ++i) {
syundo0730 23:0927e605af4b 54 buf.push_back(HOMEPOS[i]);
syundo0730 23:0927e605af4b 55 }
syundo0730 23:0927e605af4b 56 rs300->send_servo_pos(1, buf);
syundo0730 23:0927e605af4b 57 }
syundo0730 23:0927e605af4b 58
syundo0730 20:abb7852df747 59 void Controlor::read(const string& filename, uint16_t* data)
syundo0730 12:6cd135bf03bd 60 {
syundo0730 12:6cd135bf03bd 61 CSV csv;
syundo0730 20:abb7852df747 62 pose_size = new int;//<-This code is suspicious
syundo0730 13:711f74b2fa33 63 csv.read(filename, data, &servo_size, &motion_size, pose_size);
syundo0730 12:6cd135bf03bd 64 }
syundo0730 12:6cd135bf03bd 65
syundo0730 20:abb7852df747 66 void Controlor::set(uint16_t* data)
syundo0730 12:6cd135bf03bd 67 {
syundo0730 20:abb7852df747 68 int size_z = motion_size;
syundo0730 20:abb7852df747 69 int size_x = servo_size;
syundo0730 12:6cd135bf03bd 70
syundo0730 13:711f74b2fa33 71 motions = new uint16_t**[size_z];
syundo0730 12:6cd135bf03bd 72 uint16_t* p = data;
syundo0730 12:6cd135bf03bd 73
syundo0730 12:6cd135bf03bd 74 for (int i = 0; i < size_z; ++i) {
syundo0730 12:6cd135bf03bd 75 int size_y = pose_size[i];
syundo0730 12:6cd135bf03bd 76 motions[i] = new uint16_t*[size_y];
syundo0730 12:6cd135bf03bd 77 for (int j = 0; j < size_y; ++j) {
syundo0730 12:6cd135bf03bd 78 motions[i][j] = p + size_x * j;
syundo0730 12:6cd135bf03bd 79 }
syundo0730 12:6cd135bf03bd 80 p += size_x * size_y;
syundo0730 12:6cd135bf03bd 81 }
syundo0730 12:6cd135bf03bd 82 }
syundo0730 12:6cd135bf03bd 83
syundo0730 20:abb7852df747 84 bool Controlor::checkid(int id)
syundo0730 13:711f74b2fa33 85 {
syundo0730 13:711f74b2fa33 86 if (id >= 0 && id < motion_size) {
syundo0730 13:711f74b2fa33 87 return true;
syundo0730 13:711f74b2fa33 88 } else {
syundo0730 13:711f74b2fa33 89 return false;
syundo0730 13:711f74b2fa33 90 }
syundo0730 13:711f74b2fa33 91 }
syundo0730 13:711f74b2fa33 92
syundo0730 20:abb7852df747 93 void Controlor::setmotion(const int id)
syundo0730 14:522bb06f0f0d 94 {
syundo0730 20:abb7852df747 95 // TODO : Make OfflineMotion class array and attach by each id. Newing every time is not good!
syundo0730 20:abb7852df747 96 //if (!motion.playing) {
syundo0730 20:abb7852df747 97 //motion = new OfflineMotion(motions[id], pose_size[id], servo_size, pwm, &playing);
syundo0730 20:abb7852df747 98 //tick.attach(motion, &Motion::step, TIMESTEP);
syundo0730 23:0927e605af4b 99 //attached = true;
syundo0730 22:bf5aa20b9df0 100 //offline = new OfflineMotion(motions[id], pose_size[id], servo_size, pwm, &playing);
syundo0730 22:bf5aa20b9df0 101 //tick.attach(offline, &OfflineMotion::step, TIMESTEP);
syundo0730 23:0927e605af4b 102 //online = new OnlineMotion(0.5, TIMESTEP, servo_size, pwm, &playing);
syundo0730 23:0927e605af4b 103 //tick.attach(online, &OnlineMotion::step, TIMESTEP);
syundo0730 23:0927e605af4b 104 //cpg = new CPG(TIMESTEP, pwm, &playing);
syundo0730 23:0927e605af4b 105 //tick.attach(cpg, &CPG::step, TIMESTEP);
syundo0730 20:abb7852df747 106 //}
syundo0730 15:e37a8c413e51 107 }
syundo0730 14:522bb06f0f0d 108
syundo0730 20:abb7852df747 109 void Controlor::control()
syundo0730 12:6cd135bf03bd 110 {
syundo0730 23:0927e605af4b 111 if (playing) {
syundo0730 23:0927e605af4b 112 //online->step();
syundo0730 23:0927e605af4b 113 cpg->step();
syundo0730 23:0927e605af4b 114 }
syundo0730 22:bf5aa20b9df0 115 char head = comu->getheader();
syundo0730 22:bf5aa20b9df0 116 if (head == 'a') {
syundo0730 16:e65c192b7ecf 117 int id = comu->getid();
syundo0730 16:e65c192b7ecf 118 if (checkid(id)) {
syundo0730 23:0927e605af4b 119 setmotion(id);
syundo0730 16:e65c192b7ecf 120 }
syundo0730 22:bf5aa20b9df0 121 } else if (head == 'b') {
syundo0730 16:e65c192b7ecf 122 int id = comu->getid();
syundo0730 22:bf5aa20b9df0 123 uint16_t val = comu->get_int_cr();
syundo0730 22:bf5aa20b9df0 124 pwm->setDuty(id, val);
syundo0730 23:0927e605af4b 125 } else if (head == 'c') {
syundo0730 23:0927e605af4b 126 cpg->change_param(0.1, 0.1, 0.1);
syundo0730 23:0927e605af4b 127 } else if (head == 'd') {
syundo0730 23:0927e605af4b 128 playing = true;
syundo0730 23:0927e605af4b 129 } else if (head == 'e') {
syundo0730 23:0927e605af4b 130 playing = false;
syundo0730 23:0927e605af4b 131 } else if (head == 'e') {
syundo0730 23:0927e605af4b 132 comu->printf("Omega:\r\n");
syundo0730 23:0927e605af4b 133 float omega = comu->get_float_cr();
syundo0730 23:0927e605af4b 134 comu->printf("K:\r\n");
syundo0730 23:0927e605af4b 135 float k = comu->get_float_cr();
syundo0730 23:0927e605af4b 136 cpg->change_param(omega, k, 0);
syundo0730 23:0927e605af4b 137 } else if (head == 'f') {
syundo0730 23:0927e605af4b 138 home();
syundo0730 23:0927e605af4b 139 } else if (head == 'p') {
syundo0730 23:0927e605af4b 140 comu->showOnline(online);
syundo0730 23:0927e605af4b 141 }
syundo0730 23:0927e605af4b 142 }