![](/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
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?
User | Revision | Line number | New 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 | } |