main.cpp

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 #include "MODSERIAL.h"
00004 #include <vector>
00005 #include "mbed_rpc.h"
00006 
00007 #include "quadrapod_defs.h" //Includes pins and stuff
00008 #include "ArthropodIK.h"    // IK solver based on Oscar Liang. 
00009 #include "PolyServo.h"
00010 
00011 /**************************************************************************
00012               This is the Quadrupod control software.  
00013 ***************************************************************************/
00014 
00015 
00016 MODSERIAL pc(USBTX, USBRX);
00017 
00018 DigitalOut led_green(LED_GREEN);
00019 DigitalOut led_red(LED_RED);
00020 DigitalOut led_blue(LED_BLUE);
00021 
00022 PwmOut LCD_red(LED_BL_RED);
00023 PwmOut LCD_grn(LED_BL_GRN);
00024 PwmOut LCD_blu(LED_BL_BLU);
00025 
00026 float red_intensity, green_intensity, blue_intensity; 
00027 RPCVariable<float> rpc_bl_red(&red_intensity, "red_lvl");
00028 RPCVariable<float> rpc_bl_grn(&green_intensity, "green_lvl");
00029 RPCVariable<float> rpc_bl_blu(&blue_intensity, "blue_lvl");
00030 
00031 ArthropodSolver IKsolver();
00032 
00033 std::vector<DigitalOut * > digivec;
00034 
00035 void led_fade_thread(void const *args) {
00036   // Note that this function doesn't terminate, which is fine since it runs in
00037   // a thread.
00038   while (1) {
00039     // Since the internal LED is active low, inert the duty cycle.
00040     led_blue.write(1 - 0);
00041     Thread::wait(250);
00042     led_blue.write(1 - 0.25);
00043     Thread::wait(250);
00044     led_blue.write(1 - 0.5);
00045     Thread::wait(250);
00046     led_blue.write(1 - 0.75);
00047     Thread::wait(250);
00048     
00049   }
00050 }
00051 
00052 void led_blink_periodic(void const *args) {
00053   // Toggle the red LED when this function is called.
00054   led_red = !led_red;
00055 }
00056 
00057 int main() {
00058   // It's always nice to know what version is deployed.
00059   //serial.printf("Built " __DATE__ " " __TIME__ "\r\n");
00060   
00061   // Quick blink on startup.
00062   led_green = 0;  // Note that the internal LED is active low.
00063   wait(0.25);
00064   led_green = 1;
00065   wait(0.25);
00066   
00067   // Mandatory "Hello, world!".
00068   serial.printf("Hello, world!\r\n");
00069 
00070   // Start a thread running led_fade_thread().
00071   Thread ledFadeThread(led_fade_thread);
00072   
00073   // Set a timer to periodically call led_blink_periodic().
00074   RtosTimer ledBlinkTimer(led_blink_periodic);
00075   ledBlinkTimer.start(1000);
00076   char buf[256], outbuf[256];
00077 
00078   // Work is done in the threads, so main() can sleep.
00079     while (1) {
00080         Thread::wait(100);
00081         LCD_red = red_intensity;
00082         LCD_grn = green_intensity;
00083         LCD_blu = blue_intensity;   
00084         
00085         
00086         pc.gets(buf, 256);
00087         //Call the static call method on the RPC class
00088         RPC::call(buf, outbuf); 
00089         pc.printf("%s\n", outbuf);
00090         
00091         
00092     }
00093   
00094   
00095   
00096   Thread::wait(osWaitForever);
00097 }