![](/media/cache/profiles/e5815151957be36ad2085b7a1a02c5cc.jpg.50x50_q85.jpg)
Controlor for Humanoid. Walking trajectory generator, sensor reflection etc.
Dependencies: Adafruit-PWM-Servo-Driver MPU6050 RS300 mbed
Diff: Controlor.cpp
- Revision:
- 20:abb7852df747
- Parent:
- 19:c2ec475367aa
- Child:
- 21:a54bcab078ed
diff -r c2ec475367aa -r abb7852df747 Controlor.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Controlor.cpp Mon Aug 19 08:10:58 2013 +0000 @@ -0,0 +1,110 @@ +#include <iostream> +#include <string> + +#include "Controlor.h" + +Ticker tick; + +//detach may be better to controled from Controlor class + +Controlor::Controlor(uint16_t* data) : playing(false), attached(false) +{ + pwm = new PWM(); + //comu = new SCI(p28, p27); + comu = new SCI(USBTX, USBRX); + + LocalFileSystem* local = new LocalFileSystem("local"); + + read("/local/motion.csv", data); + set(data); + + // Motion 0 is Home position + setmotion(0); +} + +Controlor::~Controlor() +{ + for (int i = 0; i < motion_size; i++) { + delete[] motions[i]; + } + delete[] motions; + + delete comu; + delete pwm; +} + +void Controlor::read(const string& filename, uint16_t* data) +{ + CSV csv; + pose_size = new int;//<-This code is suspicious + csv.read(filename, data, &servo_size, &motion_size, pose_size); + //readMotion(filename, data, servo_size, motion_size, pose_size); // not so good at speed and has bug in handling float motion value. +} + +void Controlor::set(uint16_t* data) +{ + int size_z = motion_size; + int size_x = servo_size; + + motions = new uint16_t**[size_z]; + uint16_t* p = data; + + for (int i = 0; i < size_z; ++i) { + int size_y = pose_size[i]; + motions[i] = new uint16_t*[size_y]; + for (int j = 0; j < size_y; ++j) { + motions[i][j] = p + size_x * j; + } + p += size_x * size_y; + } +} + +bool Controlor::checkid(int id) +{ + if (id >= 0 && id < motion_size) { + return true; + } else { + return false; + } +} + +void Controlor::setmotion(const int id) +{ + // TODO : Make OfflineMotion class array and attach by each id. Newing every time is not good! + //if (!motion.playing) { + //motion = new OfflineMotion(motions[id], pose_size[id], servo_size, pwm, &playing); + //tick.attach(motion, &Motion::step, TIMESTEP); + attached = true; + offline = new OfflineMotion(motions[id], pose_size[id], servo_size, pwm, &playing); + tick.attach(offline, &OfflineMotion::step, TIMESTEP); + //online = new OnlineMotion(3.0, TIMESTEP, servo_size, pwm, &playing); + //tick.attach(online, &OnlineMotion::step, TIMESTEP); + //} +} + +void Controlor::control() +{ + if (!playing) { + if (attached) { + tick.detach(); + delete offline; + attached = false; + } else { + setmotion(1); + } + } + + //setmotion(1); + /*char head = comu->getheader(); + if (head == 'A') { + int id = comu->getid(); + if (checkid(id)) { + setmotion(id); + } + } else if (head == 'B') { + int id = comu->getid(); + uint16_t val = comu->getservoval(); + pwm->SetDuty(id, (uint32_t)val); + }*/ + +} \ No newline at end of file