Diff: main.cpp

Revision:

0:4f749f62c6d7

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Apr 29 15:50:23 2011 +0000
@@ -0,0 +1,335 @@
+/*
+Copyright (c) 2011 JKSOFT
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.
+*/
+
+#include "mbed.h"
+#include "USBHost.h"
+#include "Utils.h"
+#include "BD6211F.h"
+#include "Wiimote.h"
+#include "EthernetPowerControl.h"
+#include "SimpleFilter.h"
+
+// #define _SENSOR_6
+
+#define SENSOR_CYCLE        0.005f  // 5ms
+#define SENSOR_SAMPLING     0.020f  // 20ms
+#ifndef _SENSOR_6
+#define SENSOR_NUM          4
+#else
+#define SENSOR_NUM          6
+#endif
+
+#define USB_INIT_CNT_MAX    40 // 0.5s X 40 = 20s
+
+// ----- Wallbot I/O Setting ----- 
+
+// Motor
+BD6211F      RightMotor(p21,p22);
+BD6211F      LeftMotor(p23,p24);
+
+// Floor Sensor
+AnalogIn     RightSideSensor(p15);
+AnalogIn     RightCenterSensor(p16);
+AnalogIn     LeftCenterSensor(p17);
+AnalogIn     LeftSideSensor(p18);
+#ifdef _SENSOR_6
+AnalogIn    RightBackSensor(p19);
+AnalogIn    LeftBackSensor(p20);
+#endif
+
+#ifndef _SENSOR_6
+AnalogIn    _FloorSensor[] = {
+    RightSideSensor ,
+    RightCenterSensor ,
+    LeftCenterSensor ,
+    LeftSideSensor
+};
+#else
+AnalogIn    _FloorSensor[] = {
+    RightSideSensor ,
+    RightCenterSensor ,
+    LeftCenterSensor ,
+    LeftSideSensor ,
+    RightBackSensor ,
+    LeftBackSensor
+};
+#endif
+// Setting Sw
+// It is a Pull-Up
+DigitalIn    sw1(p29);
+DigitalIn    sw2(p30);
+
+// LED
+DigitalOut    chk_led(LED4);
+
+// ------------------------------- 
+
+// Input processing (Sensor & Sw)
+Ticker sensor;
+int sampling = (int)(SENSOR_SAMPLING / SENSOR_CYCLE);
+int        sw[2][10];
+
+SimpleFilter fl1(4);
+SimpleFilter fl2(4);
+SimpleFilter fl3(4);
+SimpleFilter fl4(4);
+SimpleFilter fl5(4);
+SimpleFilter fl6(4);
+SimpleFilter FloorSensor_fl[] = { fl1, fl2, fl3, fl4, fl5, fl6 };
+
+// Wii remote chk
+int wd_timer = 0;
+int usb_init_count = 0;
+
+// Timer tick(SENSOR_CYCLE[ms])
+void cycle_proc()
+{
+    static int counter = 0;
+    int i;
+    
+    if( counter >= sampling )
+    {
+        counter = 0;
+    }
+    
+    // Sw
+    sw[0][counter] = sw1;
+    sw[1][counter] = sw2;
+
+    
+    // FloorSensor
+    for( i = 0 ; i < SENSOR_NUM ; i++ )
+    {
+        FloorSensor_fl[i].filter(_FloorSensor[i].read_u16() >> 1);
+    }
+    
+    
+    counter ++;
+    wd_timer ++;
+}
+
+// 
+int GetFloorSensor(int num)
+{
+    int ret;
+    
+    if( num >= SENSOR_NUM )    return( 0 );
+    
+    ret = (int)FloorSensor_fl[num].value();
+    
+    return(ret);
+}
+
+// Sw ON:true OFF:false
+bool GetSw(int num)
+{
+    int i,sum = 0;
+    bool ret = false;
+    
+    for( i = 0 ; i < sampling ; i++ )
+    {
+        sum += sw[num][i];
+    }
+    if( sum == 0 )
+    {
+        ret = true;
+    }
+    
+    return( ret );
+}
+
+// Direct control mode
+int DirectMode( Wiimote* wii, int stat )
+{
+    int ret = stat;
+    
+    if( wii->left )
+    {
+        RightMotor = 1.0;
+        LeftMotor = -1.0;
+    }
+    else if( wii->right )
+    {
+        RightMotor = -1.0;
+        LeftMotor = 1.0;
+    }    
+    else if( wii->up )
+    {
+        RightMotor = 1.0;
+        LeftMotor = 1.0;
+    }
+    else if( wii->down )
+    {
+        RightMotor = -1.0;
+        LeftMotor = -1.0;
+    }
+    else
+    {
+        RightMotor = 0.0;
+        LeftMotor = 0.0;
+    }
+
+    float factor = wii->wheel / 150.0f; 
+    
+    float left_factor = (factor >= 0.0) ? 1.0 : 1.0 - (-factor);
+    float right_factor = (factor <= 0.0) ? 1.0 : 1.0 - factor;
+    
+    if( wii->one )
+    {
+        RightMotor = right_factor;
+        LeftMotor = left_factor;
+    }
+    if( wii->two )
+    {
+        RightMotor = -left_factor;
+        LeftMotor = -right_factor;
+    }
+    
+    return(ret);
+}
+
+// Processing when receiving it from Wiiremote
+int wall_bot_remote(char *c,int stat)
+{
+    Wiimote wii;
+    int ret = stat;
+    
+    wii.decode(c);
+    
+    ret = DirectMode( &wii ,ret );
+    
+    wd_timer = 0;
+    chk_led = 0;
+    usb_init_count = USB_INIT_CNT_MAX; 
+    
+    return(ret);
+}
+
+void input_chk(void)
+{
+    short fl[4];
+    int i;
+    bool sw[2];
+    
+    for(i=0;i<4;i++)
+    {
+        fl[i] = GetFloorSensor(i);
+    }
+    sw[0] = GetSw(0);
+    sw[1] = GetSw(1);
+    
+    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]);
+    
+}
+
+void output_chk(void)
+{
+    static int step = 0;
+    
+    switch(step)
+    {
+    case 0:
+        RightMotor = 1.0;
+        LeftMotor = 1.0;
+        break;
+    case 1:
+        RightMotor = -1.0;
+        LeftMotor = -1.0;
+        break;
+    case 2:
+        RightMotor = -1.0;
+        LeftMotor = 1.0;
+        break;
+    case 3:
+        RightMotor = 1.0;
+        LeftMotor = -1.0;
+        break;
+    }
+    step = (step+1)%4;
+}
+
+// Wii Time Out chk
+void wd_wii_chk(void)
+{
+    if( wd_timer > (0.5/SENSOR_CYCLE) )
+    {
+        chk_led = !chk_led;
+        
+        wd_timer = 0;
+        
+        RightMotor.speed(0.0);
+        LeftMotor.speed(0.0);
+        
+        usb_init_count++;
+        
+        if(usb_init_count > USB_INIT_CNT_MAX)
+        {
+            USBInit();
+            usb_init_count = 0;
+        }
+    }
+}
+
+int GetConsoleChar()
+{
+    return(0);
+}
+
+int OnDiskInsert(int device)
+{
+    return(0);
+}
+
+int main()
+{
+    // Init
+    
+    // Ether PHY Stop
+    PHY_PowerDown();
+    
+    // Sw Pull up
+    sw1.mode(PullUp);
+    sw2.mode(PullUp);
+    
+    // Motor stop
+    RightMotor.speed(0.0);
+    LeftMotor.speed(0.0);
+    
+    // USB Init is done for Bluetooth
+    USBInit();
+    usb_init_count = 0;
+    
+    // Sensing Processing
+    sensor.attach(&cycle_proc, SENSOR_CYCLE);
+
+    while(1)
+    {
+        
+       // USB Processing is done for Bluetooth
+       USBLoop();
+       wd_wii_chk();
+       
+       // input_chk();
+       // output_chk();
+       // wait(0.1);
+    }
+}