main.cpp

00001 /*
00002 Copyright (c) 2011 JKSOFT
00003 
00004 Permission is hereby granted, free of charge, to any person obtaining a copy
00005 of this software and associated documentation files (the "Software"), to deal
00006 in the Software without restriction, including without limitation the rights
00007 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00008 copies of the Software, and to permit persons to whom the Software is
00009 furnished to do so, subject to the following conditions:
00010 
00011 The above copyright notice and this permission notice shall be included in
00012 all copies or substantial portions of the Software.
00013 
00014 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00015 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00016 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00017 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00018 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00019 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00020 THE SOFTWARE.
00021 */
00022 
00023 #include "mbed.h"
00024 #include "USBHost.h"
00025 #include "Utils.h"
00026 #include "BD6211F.h"
00027 #include "Wiimote.h"
00028 #include "EthernetPowerControl.h"
00029 #include "SimpleFilter.h"
00030 
00031 // #define _SENSOR_6
00032 
00033 #define SENSOR_CYCLE        0.005f  // 5ms
00034 #define SENSOR_SAMPLING     0.020f  // 20ms
00035 #ifndef _SENSOR_6
00036 #define SENSOR_NUM          4
00037 #else
00038 #define SENSOR_NUM          6
00039 #endif
00040 
00041 #define USB_INIT_CNT_MAX    40 // 0.5s X 40 = 20s
00042 
00043 // ----- Wallbot I/O Setting ----- 
00044 
00045 // Motor
00046 BD6211F      RightMotor(p21,p22);
00047 BD6211F      LeftMotor(p23,p24);
00048 
00049 // Floor Sensor
00050 AnalogIn     RightSideSensor(p15);
00051 AnalogIn     RightCenterSensor(p16);
00052 AnalogIn     LeftCenterSensor(p17);
00053 AnalogIn     LeftSideSensor(p18);
00054 #ifdef _SENSOR_6
00055 AnalogIn    RightBackSensor(p19);
00056 AnalogIn    LeftBackSensor(p20);
00057 #endif
00058 
00059 #ifndef _SENSOR_6
00060 AnalogIn    _FloorSensor[] = {
00061     RightSideSensor ,
00062     RightCenterSensor ,
00063     LeftCenterSensor ,
00064     LeftSideSensor
00065 };
00066 #else
00067 AnalogIn    _FloorSensor[] = {
00068     RightSideSensor ,
00069     RightCenterSensor ,
00070     LeftCenterSensor ,
00071     LeftSideSensor ,
00072     RightBackSensor ,
00073     LeftBackSensor
00074 };
00075 #endif
00076 // Setting Sw
00077 // It is a Pull-Up
00078 DigitalIn    sw1(p29);
00079 DigitalIn    sw2(p30);
00080 
00081 // LED
00082 DigitalOut    chk_led(LED4);
00083 
00084 // ------------------------------- 
00085 
00086 // Input processing (Sensor & Sw)
00087 Ticker sensor;
00088 int sampling = (int)(SENSOR_SAMPLING / SENSOR_CYCLE);
00089 int        sw[2][10];
00090 
00091 SimpleFilter fl1(4);
00092 SimpleFilter fl2(4);
00093 SimpleFilter fl3(4);
00094 SimpleFilter fl4(4);
00095 SimpleFilter fl5(4);
00096 SimpleFilter fl6(4);
00097 SimpleFilter FloorSensor_fl[] = { fl1, fl2, fl3, fl4, fl5, fl6 };
00098 
00099 // Wii remote chk
00100 int wd_timer = 0;
00101 int usb_init_count = 0;
00102 
00103 // Timer tick(SENSOR_CYCLE[ms])
00104 void cycle_proc()
00105 {
00106     static int counter = 0;
00107     int i;
00108     
00109     if( counter >= sampling )
00110     {
00111         counter = 0;
00112     }
00113     
00114     // Sw
00115     sw[0][counter] = sw1;
00116     sw[1][counter] = sw2;
00117 
00118     
00119     // FloorSensor
00120     for( i = 0 ; i < SENSOR_NUM ; i++ )
00121     {
00122         FloorSensor_fl[i].filter(_FloorSensor[i].read_u16() >> 1);
00123     }
00124     
00125     
00126     counter ++;
00127     wd_timer ++;
00128 }
00129 
00130 // 
00131 int GetFloorSensor(int num)
00132 {
00133     int ret;
00134     
00135     if( num >= SENSOR_NUM )    return( 0 );
00136     
00137     ret = (int)FloorSensor_fl[num].value();
00138     
00139     return(ret);
00140 }
00141 
00142 // Sw ON:true OFF:false
00143 bool GetSw(int num)
00144 {
00145     int i,sum = 0;
00146     bool ret = false;
00147     
00148     for( i = 0 ; i < sampling ; i++ )
00149     {
00150         sum += sw[num][i];
00151     }
00152     if( sum == 0 )
00153     {
00154         ret = true;
00155     }
00156     
00157     return( ret );
00158 }
00159 
00160 // Direct control mode
00161 int DirectMode( Wiimote* wii, int stat )
00162 {
00163     int ret = stat;
00164     
00165     if( wii->left )
00166     {
00167         RightMotor = 1.0;
00168         LeftMotor = -1.0;
00169     }
00170     else if( wii->right )
00171     {
00172         RightMotor = -1.0;
00173         LeftMotor = 1.0;
00174     }    
00175     else if( wii->up )
00176     {
00177         RightMotor = 1.0;
00178         LeftMotor = 1.0;
00179     }
00180     else if( wii->down )
00181     {
00182         RightMotor = -1.0;
00183         LeftMotor = -1.0;
00184     }
00185     else
00186     {
00187         RightMotor = 0.0;
00188         LeftMotor = 0.0;
00189     }
00190 
00191     float factor = wii->wheel / 150.0f; 
00192     
00193     float left_factor = (factor >= 0.0) ? 1.0 : 1.0 - (-factor);
00194     float right_factor = (factor <= 0.0) ? 1.0 : 1.0 - factor;
00195     
00196     if( wii->one )
00197     {
00198         RightMotor = right_factor;
00199         LeftMotor = left_factor;
00200     }
00201     if( wii->two )
00202     {
00203         RightMotor = -left_factor;
00204         LeftMotor = -right_factor;
00205     }
00206     
00207     return(ret);
00208 }
00209 
00210 // Processing when receiving it from Wiiremote
00211 int wall_bot_remote(char *c,int stat)
00212 {
00213     Wiimote wii;
00214     int ret = stat;
00215     
00216     wii.decode(c);
00217     
00218     ret = DirectMode( &wii ,ret );
00219     
00220     wd_timer = 0;
00221     chk_led = 0;
00222     usb_init_count = USB_INIT_CNT_MAX; 
00223     
00224     return(ret);
00225 }
00226 
00227 void input_chk(void)
00228 {
00229     short fl[4];
00230     int i;
00231     bool sw[2];
00232     
00233     for(i=0;i<4;i++)
00234     {
00235         fl[i] = GetFloorSensor(i);
00236     }
00237     sw[0] = GetSw(0);
00238     sw[1] = GetSw(1);
00239     
00240     printf("%d\t%d\t%d\t%d\t%d\t%d\t\r\n",fl[0],fl[1],fl[2],fl[3],sw[0],sw[1]);
00241     
00242 }
00243 
00244 void output_chk(void)
00245 {
00246     static int step = 0;
00247     
00248     switch(step)
00249     {
00250     case 0:
00251         RightMotor = 1.0;
00252         LeftMotor = 1.0;
00253         break;
00254     case 1:
00255         RightMotor = -1.0;
00256         LeftMotor = -1.0;
00257         break;
00258     case 2:
00259         RightMotor = -1.0;
00260         LeftMotor = 1.0;
00261         break;
00262     case 3:
00263         RightMotor = 1.0;
00264         LeftMotor = -1.0;
00265         break;
00266     }
00267     step = (step+1)%4;
00268 }
00269 
00270 // Wii Time Out chk
00271 void wd_wii_chk(void)
00272 {
00273     if( wd_timer > (0.5/SENSOR_CYCLE) )
00274     {
00275         chk_led = !chk_led;
00276         
00277         wd_timer = 0;
00278         
00279         RightMotor.speed(0.0);
00280         LeftMotor.speed(0.0);
00281         
00282         usb_init_count++;
00283         
00284         if(usb_init_count > USB_INIT_CNT_MAX)
00285         {
00286             USBInit();
00287             usb_init_count = 0;
00288         }
00289     }
00290 }
00291 
00292 int GetConsoleChar()
00293 {
00294     return(0);
00295 }
00296 
00297 int OnDiskInsert(int device)
00298 {
00299     return(0);
00300 }
00301 
00302 int main()
00303 {
00304     // Init
00305     
00306     // Ether PHY Stop
00307     PHY_PowerDown();
00308     
00309     // Sw Pull up
00310     sw1.mode(PullUp);
00311     sw2.mode(PullUp);
00312     
00313     // Motor stop
00314     RightMotor.speed(0.0);
00315     LeftMotor.speed(0.0);
00316     
00317     // USB Init is done for Bluetooth
00318     USBInit();
00319     usb_init_count = 0;
00320     
00321     // Sensing Processing
00322     sensor.attach(&cycle_proc, SENSOR_CYCLE);
00323 
00324     while(1)
00325     {
00326         
00327        // USB Processing is done for Bluetooth
00328        USBLoop();
00329        wd_wii_chk();
00330        
00331        // input_chk();
00332        // output_chk();
00333        // wait(0.1);
00334     }
00335 }