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 "Wiimote.h"
00027 #include "motordriver.h"
00028 
00029 AnalogIn L_IR1(p15);      // Analog In Pin
00030 AnalogIn L_IR2(p16);      // Analog In Pin
00031 AnalogIn L_IR3(p17);      // Analog In Pin
00032 AnalogIn R_IR3(p18);      // Analog In Pin
00033 AnalogIn R_IR2(p19);      // Analog In Pin
00034 AnalogIn R_IR1(p20);      // Analog In Pin
00035 
00036 DigitalOut LLED(p7);   // Digital Out Pin
00037 DigitalOut RLED(p8);
00038 
00039 DigitalOut L_IRLED1(p9);    // Digital Out Pin
00040 DigitalOut L_IRLED2(p10);   // Digital Out Pin
00041 DigitalOut L_IRLED3(p11);   // Digital Out Pin
00042 DigitalOut R_IRLED3(p12);   // Digital Out Pin
00043 DigitalOut R_IRLED2(p13);   // Digital Out Pin
00044 DigitalOut R_IRLED1(p14);   // Digital Out Pin
00045 
00046 Motor L_Motor(p25, p22, p21, 1); // pwm, fwd, rev, can break
00047 Motor R_Motor(p26, p24, p23, 1); // pwm, fwd, rev, can break
00048 
00049 
00050 // Direct control mode
00051 int DirectMode( Wiimote* wii, int stat )
00052 {
00053     LLED=1; 
00054     RLED=1; // LED Init    
00055     
00056     int ret = stat;
00057     
00058     if( wii->left )
00059     {
00060         L_Motor.speed(-1.0);
00061         R_Motor.speed(1.0);
00062         LLED = 1; RLED = 0;  
00063     }
00064     else if( wii->right )
00065     {
00066         L_Motor.speed(1.0);
00067         R_Motor.speed(-1.0);
00068         LLED = 0; RLED = 1;
00069     }    
00070     else if( wii->up )
00071     {
00072         L_Motor.speed(1.0);
00073         R_Motor.speed(1.0);
00074         LLED = 1; RLED = 1;  
00075     }
00076     else if( wii->down )
00077     {
00078         L_Motor.speed(-1.0);
00079         R_Motor.speed(-1.0);
00080         LLED = 0; RLED = 0;  
00081     }
00082     else
00083     {
00084         L_Motor.stop(0);
00085         R_Motor.stop(0);
00086     }
00087 
00088     float factor = wii->wheel / 150.0f; 
00089     
00090     float left_factor = (factor >= 0.0) ? 1.0 : 1.0 - (-factor);
00091     float right_factor = (factor <= 0.0) ? 1.0 : 1.0 - factor;
00092     
00093     if( wii->one )
00094     {
00095         L_Motor.speed(left_factor);
00096         R_Motor.speed(right_factor);
00097     }
00098     if( wii->two )
00099     {
00100         L_Motor.speed(-right_factor);
00101         R_Motor.speed(-left_factor);
00102     }
00103     
00104     return(ret);
00105 }
00106 
00107 // Processing when receiving it from Wiiremote
00108 int wall_bot_remote(char *c,int stat)
00109 {
00110     Wiimote wii;
00111     int ret = stat;
00112     
00113     wii.decode(c);
00114     
00115     ret = DirectMode( &wii ,ret );
00116     
00117     return(ret);
00118 }
00119 
00120 int GetConsoleChar()
00121 {
00122     return(0);
00123 }
00124 
00125 int OnDiskInsert(int device)
00126 {
00127     return(0);
00128 }
00129 
00130 int main()
00131 {
00132     // USB Init is done for Bluetooth
00133     USBInit();
00134     
00135     while(1)
00136     {
00137        // USB Processing is done for Bluetooth
00138        USBLoop();
00139     }
00140 }