main.cpp

00001 #include "mbed.h"
00002 #include "EncoderCounter.h"
00003 #include "IMU.h"
00004 #include "Controller.h"
00005 #include "SerialCom.h"
00006 
00007 using namespace std;
00008 
00009 class Main
00010 {
00011 
00012 public:
00013 
00014     Main();
00015     virtual     ~Main();
00016 
00017 private:
00018 
00019     static const uint32_t   STACK_SIZE = 8192;  // stack size of thread, given in [bytes]
00020 
00021     Thread      thread;
00022     void        run();
00023 };
00024 
00025 Main::Main() : thread(osPriorityNormal, STACK_SIZE)
00026 {
00027 
00028     thread.start(callback(this, &Main::run));
00029 }
00030 
00031 Main::~Main() {}
00032 
00033 void Main::run()
00034 {
00035 
00036     DigitalOut myled(LED1);
00037     Serial pc1(USBTX, USBRX); // tx, rx
00038 
00039     DigitalOut led1(PB_13);
00040     DigitalOut led2(PC_8);
00041     DigitalOut led3(PC_9);
00042     DigitalOut led4(PB_14); //neu
00043     DigitalOut led5(PB_15); // neu
00044     DigitalOut led6(PH_1);
00045     DigitalOut led7(PC_2);
00046     DigitalOut led8(PC_3);
00047     DigitalOut led9(PB_0);
00048 
00049 // create motor control objects
00050     DigitalOut enable1(PB_1);
00051     DigitalOut enable2(PB_2);
00052     DigitalOut enable3(PB_3);
00053 
00054     PwmOut pwm1(PA_8);
00055     PwmOut pwm2(PA_9);
00056     PwmOut pwm3(PA_10);
00057 
00058 // crete Encoder read objects
00059     EncoderCounter counter1(PA_6, PC_7);
00060     EncoderCounter counter2(PB_6, PB_7);
00061     EncoderCounter counter3(PA_1, PA_0);
00062 
00063 // create IMU comunication objects
00064     SPI spi(PC_12, PC_11, PC_10); // mosi, miso, sclk
00065     DigitalOut csAG(PA_15);
00066     DigitalOut csM(PD_2);
00067 
00068 // escon I/O
00069 
00070 
00071     AnalogIn M1_AOUT1(PC_4); // neu
00072     AnalogIn M1_AOUT2(PA_5); // neu
00073 
00074     AnalogIn M2_AOUT1(PC_1); // neu
00075     AnalogIn M2_AOUT2(PC_0); // neu
00076 
00077     AnalogIn M3_AOUT1(PA_7); // neu
00078     AnalogIn M3_AOUT2(PA_4); // neu
00079 
00080 
00081     Thread thread;
00082 
00083 //IMU
00084     IMU imu(spi, csAG, csM);
00085 
00086     enable1 = 1;
00087     enable2 = 1;
00088     enable3 = 1;
00089 
00090     // create serial server object
00091 
00092     RawSerial serial(PB_10, PC_5);
00093     serial.baud(38400);
00094     serial.format(8, SerialBase::None, 1);
00095     pc1.printf("init serilCom\r\n");
00096     SerialCom com(serial);
00097     pc1.printf("init serilCom done\r\n");
00098     pc1.printf("init Controller\r\n");
00099     //controller
00100     Controller controller(pwm1,pwm2,pwm3,counter1,counter2,counter3,imu);
00101     pc1.printf("init Controller done\r\n");
00102 
00103     led1 = 0;
00104     led2 = 0;
00105     led3 = 0;
00106     led4 = 1;
00107 
00108     int i = 1;
00109 
00110 
00111 
00112     int t = 0;
00113 
00114     // enter main loop
00115     pc1.printf("init done\r\n");
00116     while (true) {
00117 
00118         //pc1.printf("main\r\n");
00119 
00120 
00121         if (com.dataRecived) {
00122             led9 = 1;
00123             controller.gainG = (1.0f+com.gainG);
00124             controller.gain_dG = (1.0f+com.gain_dG);
00125             controller.offsetX = (1.0f+com.offsetX);
00126             controller.offsetY = (1.0f+com.offsetY);
00127             com.dataRecived = false;
00128             pc1.printf("gainG = %.7f gain_dG = %.7f offsetX = %.7f offsetY = %.7f\r\n",controller.gainG,controller.gain_dG,controller.offsetX,controller.offsetY);
00129             Thread::wait(50.0f);
00130         } else {
00131             led9 = 0;
00132 
00133         }
00134 
00135         led7 = !led7;
00136 
00137         if(M1_AOUT1.read() > M1_AOUT2.read()) {
00138             led1 = 1;
00139         } else {
00140             led1 = 0;
00141         }
00142         if(M2_AOUT1.read() > M2_AOUT2.read()) {
00143             led2 = 1;
00144         } else {
00145             led2 = 0;
00146         }
00147         if(M3_AOUT1.read() > M3_AOUT2.read()) {
00148             led3 = 1;
00149         } else {
00150             led3 = 0;
00151         }
00152 
00153 
00154         // security condition
00155         if(abs(imu.getGammaX()) > 0.43f || abs(imu.getGammaY()) > 0.43f) {
00156             enable1 = 0;
00157             enable2 = 0;
00158             enable3 = 0;
00159             }
00160 
00161 
00162         Thread::wait(50.0f);
00163     }
00164 }
00165 
00166 /**
00167  * This is the main program of the ROME2RevG firmware.
00168  */
00169 int32_t main()
00170 {
00171 
00172     Main main;
00173 
00174     Thread::wait(osWaitForever);
00175 }
00176