This program contains a class that makes it easy for the mbed to communicate with the Mini SSC II or the Pololu Maestro in SSC compatibility mode. (they are servo/motor controllers)
minissc.cpp@1:8f9d24a87600, 2011-10-16 (annotated)
- Committer:
- avbotz
- Date:
- Sun Oct 16 05:08:27 2011 +0000
- Revision:
- 1:8f9d24a87600
- Parent:
- 0:399d7ca2d0bb
- Child:
- 2:e5458113c26b
Added some things here and there to make it work better. Mostly, the things are comments.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
avbotz |
1:8f9d24a87600 | 1 | /* |
avbotz |
1:8f9d24a87600 | 2 | * Demo of communicating with Mini SSC 2. |
avbotz |
1:8f9d24a87600 | 3 | */ |
avbotz |
1:8f9d24a87600 | 4 | |
avbotz |
1:8f9d24a87600 | 5 | #include "mbed.h" |
avbotz |
1:8f9d24a87600 | 6 | #include "minissc.h" |
avbotz |
1:8f9d24a87600 | 7 | |
avbotz |
1:8f9d24a87600 | 8 | // MiniSSC2's Constructor |
avbotz |
1:8f9d24a87600 | 9 | MiniSSC2::MiniSSC2(int a, int baud) { |
avbotz |
1:8f9d24a87600 | 10 | num_motors = a; |
avbotz |
1:8f9d24a87600 | 11 | p_device = new Serial(p9, p10); // (tx, rx) opens up new serial device (p_device is Serial* pointer |
avbotz |
1:8f9d24a87600 | 12 | p_device->baud(baud); // Set the baud. |
avbotz |
1:8f9d24a87600 | 13 | |
avbotz |
1:8f9d24a87600 | 14 | set(127); |
avbotz |
1:8f9d24a87600 | 15 | } |
avbotz |
1:8f9d24a87600 | 16 | |
avbotz |
1:8f9d24a87600 | 17 | // MiniSSC2's Destructor |
avbotz |
1:8f9d24a87600 | 18 | MiniSSC2::~MiniSSC2() { |
avbotz |
1:8f9d24a87600 | 19 | if (p_device != NULL) { |
avbotz |
1:8f9d24a87600 | 20 | delete p_device; // must do this. otherwise, you'll have memory leakage & you may not be able to re-open the serial port |
avbotz |
1:8f9d24a87600 | 21 | } |
avbotz |
1:8f9d24a87600 | 22 | } |
avbotz |
1:8f9d24a87600 | 23 | |
avbotz |
1:8f9d24a87600 | 24 | void MiniSSC2::send() { |
avbotz |
1:8f9d24a87600 | 25 | for (int i = 0; i < num_motors; i++) { |
avbotz |
1:8f9d24a87600 | 26 | send(i); |
avbotz |
1:8f9d24a87600 | 27 | } |
avbotz |
1:8f9d24a87600 | 28 | } |
avbotz |
1:8f9d24a87600 | 29 | |
avbotz |
1:8f9d24a87600 | 30 | void MiniSSC2::send(int i_motor) { |
avbotz |
1:8f9d24a87600 | 31 | // format: {sync byte, motor id, motor power} |
avbotz |
1:8f9d24a87600 | 32 | // example: {255, 2, 24} |
avbotz |
1:8f9d24a87600 | 33 | p_device->putc(SSC_SYNC_BYTE); |
avbotz |
1:8f9d24a87600 | 34 | p_device->putc((char)i_motor); |
avbotz |
1:8f9d24a87600 | 35 | p_device->putc(motors[i_motor-1]); |
avbotz |
1:8f9d24a87600 | 36 | } |
avbotz |
1:8f9d24a87600 | 37 | |
avbotz |
1:8f9d24a87600 | 38 | void MiniSSC2::set(char value) { |
avbotz |
1:8f9d24a87600 | 39 | for (int i = 1; i <= num_motors; i++) { |
avbotz |
1:8f9d24a87600 | 40 | set(i, value); |
avbotz |
1:8f9d24a87600 | 41 | } |
avbotz |
1:8f9d24a87600 | 42 | } |
avbotz |
1:8f9d24a87600 | 43 | |
avbotz |
1:8f9d24a87600 | 44 | void MiniSSC2::set(int i_motor, char value) { |
avbotz |
1:8f9d24a87600 | 45 | motors[i_motor-1] = value; |
avbotz |
1:8f9d24a87600 | 46 | } |
avbotz |
1:8f9d24a87600 | 47 | |
avbotz |
1:8f9d24a87600 | 48 | char MiniSSC2::get(int i_motor) { |
avbotz |
1:8f9d24a87600 | 49 | return motors[i_motor-1]; |
avbotz |
1:8f9d24a87600 | 50 | } |
avbotz |
1:8f9d24a87600 | 51 | |
avbotz |
1:8f9d24a87600 | 52 | MiniSSC2 ssc(4, 9600); // 4 motors, baud for MiniSSC2 is 9600 |
avbotz |
1:8f9d24a87600 | 53 | Ticker ssc_to; |
avbotz |
1:8f9d24a87600 | 54 | |
avbotz |
1:8f9d24a87600 | 55 | void ssc_send_cb() { |
avbotz |
1:8f9d24a87600 | 56 | ssc.send(); // is there a more efficient way to do this? |
avbotz |
1:8f9d24a87600 | 57 | } |
avbotz |
1:8f9d24a87600 | 58 | |
avbotz |
1:8f9d24a87600 | 59 | // MAIN FUNCTION ============================================================== |
avbotz |
1:8f9d24a87600 | 60 | int main() { |
avbotz |
1:8f9d24a87600 | 61 | ssc_to.attach(&ssc_send_cb, 0.002); // run ssc_send_cb() every 0.002 seconds (Ticker) |
avbotz |
1:8f9d24a87600 | 62 | |
avbotz |
1:8f9d24a87600 | 63 | while (true) { |
avbotz |
1:8f9d24a87600 | 64 | ssc.set(127); |
avbotz |
1:8f9d24a87600 | 65 | } |
avbotz |
1:8f9d24a87600 | 66 | } |
avbotz |
1:8f9d24a87600 | 67 | // Don't delete this comment. |