main.cpp

00001 #include "mbed.h"
00002 #include <stdio.h>
00003 #include <stdlib.h>
00004 #include <math.h> 
00005 #include "StewartPlatform.h"
00006 DigitalOut myled(LED1);
00007 Serial pc(SERIAL_TX, SERIAL_RX);
00008 StewartPlatformType Platform;                                           //动感平台数据结构体 
00009 
00010 int main() 
00011 {
00012     while(1) 
00013     {
00014         myled = 1; // LED is ON
00015         wait(0.2); // 200 ms
00016         myled = 0; // LED is OFF
00017         wait(1.0); // 1 sec
00018         
00019         
00020         Platform.topRadius = 244.95;                                            //平台参数初始化 
00021         Platform.topInterval = 100;
00022         Platform.bottomRadius = 332.54;
00023         Platform.bottomInterval = 340;
00024         Platform.lengthOfSteelWheel = 150;
00025         Platform.lengthOfCardan = 150;
00026         Platform.lengthOfBar = 368;
00027         Platform.x = 25;                                                        //设定上平台姿态 
00028         Platform.y = 31;
00029         Platform.z = 278;
00030         Platform.a = 15;
00031         Platform.b = 12;
00032         Platform.c = 22;
00033         CalStewartPlatform(&Platform);                                          //解析平台数据 
00034         pc.printf("Angle: %.2f %.2f %.2f %.2f %.2f %.2f \r\n", Platform.theta[0], Platform.theta[1], Platform.theta[2], Platform.theta[3], Platform.theta[4], Platform.theta[5]);
00035         pc.printf("Servo Angle: %.2f %.2f %.2f %.2f %.2f %.2f \r\n", Platform.theta_servo[0], Platform.theta_servo[1], Platform.theta_servo[2], Platform.theta_servo[3], Platform.theta_servo[4], Platform.theta_servo[5]);
00036         pc.printf("BarLength: %.2f %.2f %.2f %.2f %.2f %.2f \r\n", Platform.BarLength[0], Platform.BarLength[1], Platform.BarLength[2], Platform.BarLength[3], Platform.BarLength[4], Platform.BarLength[5]);
00037     }
00038 }