Code for a quadrapod robot.
Dependencies: ArthropodIK MMA8451Q MODSERIAL TSI TextLCD mbed-rtos mbed PolyServo
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 }
Generated on Sat Jul 16 2022 03:19:41 by 1.7.2