minissc.cpp

00001 /*
00002  * Demo that communicates with the MiniSSC 2. This also works with the Pololu Mini Maestro.
00003  */
00004 
00005 #include "mbed.h"
00006 #include "minissc.h"
00007 
00008 // MiniSSC2's Constructor
00009 MiniSSC2::MiniSSC2(int num_motors, int baud, PinName tx, PinName rx) {
00010     this->num_motors = num_motors;
00011     p_device = new Serial(tx, rx); // (tx, rx) opens up new serial device (p_device is Serial* pointer)
00012     p_device->format(8, Serial::None, 1);
00013     p_device->baud(baud); // Set the baud.
00014     set(127);   // The motors should start stationary (zero power)
00015 }
00016 
00017 // MiniSSC2's Destructor
00018 MiniSSC2::~MiniSSC2() {
00019     if (p_device != NULL) {
00020         // must do this. otherwise, you'll have memory leakage & you may not be able to re-open the serial port later
00021         delete p_device;
00022     }
00023 }
00024 
00025 void MiniSSC2::send() {
00026     for (int i = 0; i < num_motors; i++) {
00027         send(i);
00028     }
00029 }
00030 
00031 void MiniSSC2::send(int i_motor) {
00032     // format: {sync byte, motor id, motor power}
00033     // example: {SSC_SYNC_BYTE, 2, 24} sets motor 2 to power level 24
00034     p_device->putc(SSC_SYNC_BYTE);
00035     p_device->putc((unsigned char)i_motor);
00036     p_device->putc(motors[i_motor]);
00037 }
00038 
00039 void MiniSSC2::set(unsigned char value) {
00040     for (int i = 0; i < num_motors; i++) {
00041         set(i, value);
00042     }
00043 }
00044 
00045 void MiniSSC2::set(int i_motor, unsigned char value) {
00046     motors[i_motor] = value;
00047 }
00048 
00049 unsigned char MiniSSC2::get(int i_motor) {
00050     return motors[i_motor];
00051 }
00052 
00053 void ssc_send_cb() {
00054     PC.printf("Sent (callback)\n");
00055     ssc.send(); // is there a more efficient way to do this?
00056 }
00057 
00058 void led1_on_cb()
00059 {
00060     led1 = 1;
00061 }
00062 
00063 void led1_off_cb()
00064 {
00065     led1 = 0;
00066 }
00067 
00068 /* MAIN FUNCTION */
00069 // this code is very bad. please do not use it for anything serious.
00070 int main() {
00071     led1 = 1;
00072     wait_ms(500);
00073     //ssc_to.attach(&ssc_send_cb, 1); // run ssc_send_cb() at 10 Hz (Ticker) (increase if possible)
00074     PC.baud(115200);
00075     PC.printf("\n\r\n\rHello.\n\r");
00076     
00077     led1_on.attach(&led1_on_cb, 1.0f);
00078     wait_ms(50);
00079     led1_off.attach(&led1_off_cb, 1.0f);
00080     
00081         ssc.set(1);
00082     while (true)
00083     {
00084         led2 = led3 = led4 = 0;
00085         led2 = 1;
00086         ssc.send();
00087         /*PC.printf("Motors: l = left, r = right, f = forward, b = back.\n\r");
00088         PC.printf("Which motor? ");
00089         int in = PC.getc();
00090         PC.printf("%c\n\r", in);
00091         char motorID, motorData;
00092         PC.printf("Setting motor ");
00093         switch (in)
00094         {
00095             case 'r':
00096                 motorID = 0;
00097                 PC.printf("left.\n\r");
00098                 break;
00099                 
00100             case 'l':
00101                 motorID = 1;
00102                 PC.printf("right.\n\r");
00103                 break;
00104             
00105             case 'f':
00106                 motorID = 2;
00107                 PC.printf("front.\n\r");
00108                 break;
00109             
00110             case 'b':
00111                 motorID = 3;
00112                 PC.printf("back.\n\r");
00113                 break;
00114                 
00115             case 'q':
00116                 ssc.set(127);
00117                 ssc.send();
00118                 break;
00119                 
00120             default:
00121                 PC.printf("Invalid motor. Try again.\n\r");
00122                 continue;
00123                 break;
00124         }
00125         led3 = 1;
00126         PC.printf("Power (-100 to 100): ");
00127         std::string powerRaw;
00128         tryAgain:
00129         powerRaw.clear();
00130         while (true)
00131         {
00132             in = PC.getc();
00133             PC.putc(in);
00134             led4 = 1;
00135             if (in == '\r')
00136             {
00137                 PC.putc('\n');
00138                 break;
00139             }
00140             powerRaw.push_back(in);
00141         }
00142         
00143         if (sscanf(powerRaw.c_str(), "%d", &in) < 1 || in > 100 || in < -100)
00144         {
00145             printf("Invalid power. Try again: ");
00146             goto tryAgain;
00147         }
00148         
00149         in *= 1.27f;
00150         in += 127;
00151         
00152         //PC.printf("Set motor %d to %d\n\r\n\r", motorID, in);
00153         
00154         ssc.set(motorID, in);
00155         ssc.send(motorID);
00156         
00157         PC.printf("\n\r--------------------------------\n\r");*/
00158     }
00159 }