main.cpp

00001 #include "mbed.h"
00002 #include "platform/mbed_thread.h"
00003 #include "Controller.h"
00004 
00005 #define INT_TIME 0.001
00006 #define INT_TIME_MS INT_TIME*1000
00007 
00008 
00009 RawSerial sabertooth1(p21,NC,115200);//左前　出力
00010 RawSerial sabertooth1(p23,NC,115200);//左後　出力
00011 RawSerial sabertooth2(p24,NC,115200);//右後　出力
00012 RawSerial sabertooth2(p22,NC,115200);//右前　出力
00013 DigitalOut air(p25);//エアシリンダーのつもり
00014 
00015 Serial pc(USBTX, USBRX,115200);
00016 Controller con(p9,p10,115200); //TXpin, RXpin, baudrateを設定
00017 //Controller con(p13,p14,115200); //TXpin, RXpin, baudrateを設定
00018 //DigitalOut led_user(LED_USER);
00019 //DigitalOut led_red(LED_RED);
00020 //DigitalOut pin_emergency(D0); //非常停止信号
00021 
00022 Ticker interrupt;
00023 bool flag_10ms = false;
00024 bool flag_1s = false;
00025 
00026 void interrupt_func()
00027 {
00028     static int count_10ms = 0;
00029     if(count_10ms++ > (INT_TIME_MS*10 - 1))
00030     {
00031         flag_10ms = true;
00032         count_10ms = 0;
00033         
00034         static int count_1s = 0;
00035         if(count_1s++ > (INT_TIME_MS*100 -1))
00036         {
00037             flag_1s = true;
00038             count_1s = 0;
00039         }
00040     }
00041 }
00042 
00043 int main()
00044 {   
00045     //pc.baud(115200);
00046     
00047     /* 第一引数はタイムアウト時間[ms]，第二引数はupdate関数の呼び出し周期[ms] */
00048     con.init(1000, INT_TIME_MS*10); //init関数を呼び出さなければタイムアウトの処理は行われない（available関数は常にtrueを返す）
00049     
00050     interrupt.attach(&interrupt_func, INT_TIME);
00051     
00052     while(1) 
00053     {
00054         //pc.printf("");
00055         if(flag_10ms)
00056         {
00057             con.update(); //main関数のflag内で呼び出す．
00058             if(con.available())
00059             {
00060                 int buttonState = con.getButtonState();
00061                 uint8_t joyRy = con.readJoyRYbyte();
00062                 uint8_t joyLx = con.readJoyLXbyte();
00063                 uint8_t joyLy = con.readJoyLYbyte();
00064                 
00065                 pc.printf("%u\t", buttonState);
00066                 pc.printf("%u\t", joyRy);
00067                 pc.printf("%u\t", joyLx);
00068                 pc.printf("%u\r", joyLy);
00069                 
00070                 switch(buttonState)
00071                 {
00072                     case 8:
00073                         air.write(1);
00074                         break;
00075                     default:
00076                         air.write(0);
00077                         break;
00078                 }
00079                 
00080                 double LF=0;
00081                 double RF=0;
00082                 double LB=0;
00083                 double RB=0;
00084                 
00085                 LF=joyLx-joyLy-joyRy+128;//上と左が正　下と右が負 Yがスティック左右方向　Xがスティック上下方向
00086                 RF=-joyLx-joyLy-joyRy+384;
00087                 LB=joyLx+joyLy-joyRy-128;
00088                 RB=-joyLx+joyLy-joyRy+128;
00089                 
00090                 sabertooth1.printf("M1:%d\r\n",LF);
00091                 sabertooth1.printf("M2:%d\r\n",LB);
00092                 sabertooth2.printf("M1:%d\r\n",RB);
00093                 sabertooth2.printf("M2:%d\r\n",RF);
00094                 
00095                 pc.printf("%lf\t", LF);
00096                 pc.printf("%lf\t", RF);
00097                 pc.printf("%lf\t", LB);
00098                 pc.printf("%lf\r\n", RB);
00099                 //led_red.write(1);
00100                 //pin_emergency.write(1); //非常停止を解除する
00101             }
00102             else
00103             {
00104                 pc.printf("disconnected\r\n");
00105                 //led_red.write(0);
00106                 //pin_emergency.write(0); //非常停止を作動させる
00107             }
00108             
00109             flag_10ms = false;
00110         }
00111         
00112         if(flag_1s)
00113         {
00114             //led_user.write(!led_user.read());
00115             flag_1s = false;
00116         }
00117         wait_us(1);
00118         //thread_sleep_for(1);
00119     }
00120 }