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@16:e65c192b7ecf, 2013-02-27 (annotated)

Committer:
syundo0730
Date:
Wed Feb 27 12:32:44 2013 +0000
Revision:
16:e65c192b7ecf
Parent:
15:e37a8c413e51
Child:
17:60de3bfdc70b
ROBO-ONE 22th version

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