Diff: main.cpp

Revision:

0:269589d8d2c2

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Nov 17 13:22:00 2012 +0000
@@ -0,0 +1,339 @@
+/*
+Copyright (c) 2012 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 "Wiimote.h"
+#include "HighSpeedAnalogIn.h"
+#include "EthernetPowerControl.h"
+#include "TB6612.h"
+
+#if 0
+#define DBG(x)    x
+#else
+#define DBG(x)
+#endif
+
+// PID terms
+#define P_TERM 1
+#define I_TERM 0
+#define D_TERM 20
+
+#define MAX 1.0
+#define MIN -1.0
+
+#define MAX_SPEED 100
+
+Serial pc(USBTX, USBRX);
+BusOut myleds(LED1, LED2, LED3, LED4);
+BusOut myleds2(p7,p8,p9,p10);
+
+// ----- Wallbot I/O Setting ----- 
+// Motor
+TB6612 right(p21,p12,p11);
+TB6612 left(p22,p14,p13);
+
+HighSpeedAnalogIn ain(p15, p16, p17, p18, p19, p20);
+
+Ticker flipper;
+int com_time_out = 0;
+int com_stat = 0;
+int move_time = 0;
+int lmp = 0,kind = 0;
+
+extern "C" void mbed_reset();
+
+//      p20 p19 p18 p17
+// LEFT  o   o   o   o  RIGHT
+float GetSensor(int sh)
+{
+    float ret = 0.0;
+    int bit = 0;
+    int value[4];
+    
+    value[0] = ain.read_u16(p17);
+    value[1] = ain.read_u16(p18);
+    value[2] = ain.read_u16(p19);
+    value[3] = ain.read_u16(p20);
+    
+    if( value[0] > sh ) bit |= 0x01;
+    if( value[1] > sh ) bit |= 0x02;
+    if( value[2] > sh ) bit |= 0x04;
+    if( value[3] > sh ) bit |= 0x08;
+    
+    myleds = bit;
+    
+    switch(bit)
+    {
+    case 0x01:    ret = 1.0;        break;
+    case 0x03:    ret = 0.66;        break;
+    case 0x02:    ret = 0.33;        break;
+    case 0x04:    ret = -0.33;    break;
+    case 0x0C:    ret = -0.66;    break;
+    case 0x08:    ret = -1.0;        break;
+    default:    ret = 0.0;        break;
+    }
+    
+  //  DBG(printf("[SENSOR] %d\t %d\t %d\t %d\t [%02X] : %f\n",value[0],value[1],value[2],value[3],bit,ret);)
+    
+    return(ret);
+}
+
+float GetSensor(int sh, int *stat)
+{
+    float ret = 0.0;
+    int bit = 0;
+    int value[4];
+    
+    value[0] = ain.read_u16(p17);
+    value[1] = ain.read_u16(p18);
+    value[2] = ain.read_u16(p19);
+    value[3] = ain.read_u16(p20);
+    
+    if( value[0] > sh ) bit |= 0x01;
+    if( value[1] > sh ) bit |= 0x02;
+    if( value[2] > sh ) bit |= 0x04;
+    if( value[3] > sh ) bit |= 0x08;
+    
+    myleds = bit;
+    *stat = bit;
+    
+    switch(bit)
+    {
+    case 0x01:    ret = 1.0;        break;
+    case 0x03:    ret = 0.66;        break;
+    case 0x02:    ret = 0.33;        break;
+    case 0x04:    ret = -0.33;    break;
+    case 0x0C:    ret = -0.66;    break;
+    case 0x08:    ret = -1.0;        break;
+    default:    ret = 0.0;        break;
+    }
+    
+  //  DBG(printf("[SENSOR] %d\t %d\t %d\t %d\t [%02X] : %f\n",value[0],value[1],value[2],value[3],bit,ret);)
+    
+    return(ret);
+}
+
+void flip() {
+    com_time_out++;
+    static int led_count = 0;
+    
+    if(com_stat == 1)
+    {
+        if(com_time_out > 2)
+        {
+            right = 0.0;
+            left = 0.0;
+            mbed_reset();
+        }
+    }
+    else
+    {
+        myleds = !myleds;
+    }
+    if(com_time_out > 150)
+    {
+        right = 0.0;
+        left = 0.0;
+        mbed_reset();
+    }
+    if(move_time != 0)
+    {
+        move_time--;
+    }
+    
+    switch(led_count)
+    {
+    case 0:
+        myleds2 = 1;
+        led_count++;
+        break;
+    case 1:
+        myleds2 = 2;
+        led_count++;
+        break;
+    case 2:
+        myleds2 = 4;
+        led_count++;
+        break;
+    case 3:
+        myleds2 = 8;
+        led_count++;
+        break;
+    case 4:
+        myleds2 = 4;
+        led_count++;
+        break;
+    case 5:
+        myleds2 = 2;
+        led_count=0;
+        break;
+    }
+
+}
+
+int LineFollowMode()
+{
+    float line_pos;
+    float derivative,proportional,power;
+    float speed =0.7;
+
+    float right_v;
+    float left_v;
+
+    static float old_line_pos = 0.0;
+    static float integral = 0.0;
+
+    
+    line_pos = GetSensor(2500);
+    
+    proportional = line_pos;
+    integral += line_pos;
+    derivative = line_pos - old_line_pos;
+    old_line_pos = line_pos;
+    
+    power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
+    
+    right_v = speed-power;
+    left_v  = speed+power;
+    
+    // limit checks
+    if (right_v < MIN)
+        right_v = MIN;
+    else if (right_v > MAX)
+        right_v = MAX;
+        
+    if (left_v < MIN)
+        left_v = MIN;
+    else if (left_v > MAX)
+        left_v = MAX;
+    
+    left = left_v;
+    right = right_v;
+    
+    return(0);
+}
+
+// Direct control mode
+int DirectMode( Wiimote* wii, int stat )
+{
+    float line_pos;
+    int ret = stat;
+    
+    if( move_time == 0 )
+    {
+        if( wii->left )
+        {
+            right = -MAX_SPEED;
+            left = MAX_SPEED;
+        }
+        else if( wii->right )
+        {
+            right = MAX_SPEED;
+            left = -MAX_SPEED;
+        }    
+        else if( wii->up )
+        {
+            right = MAX_SPEED;
+            left = MAX_SPEED;
+        }
+        else if( wii->down )
+        {
+            right = -MAX_SPEED;
+            left = -MAX_SPEED;
+        }
+        else
+        {
+            right = 0;
+            left = 0;
+        }
+    
+        float factor = wii->wheel * 1.5f;
+        
+        if(factor > 100.0f )    factor = 100.0f; 
+        if(factor < -100.0f )    factor = -100.0f;
+        
+        printf("%f\t%f\r\n",wii->wheel,factor);
+        
+        int left_factor = (int)((factor <= 0.0) ? 100.0 : 100.0 - factor);
+        int right_factor = (int)((factor >= 0.0) ? 100.0 : 100.0 - (-factor));
+        
+        if( wii->one )
+        {
+            right = right_factor;
+            left = left_factor;
+        }
+        if( wii->two )
+        {
+            right = -left_factor;
+            left = -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 );
+    
+    return(ret);
+}
+
+int GetConsoleChar()
+{
+    return(0);
+}
+
+int OnDiskInsert(int device)
+{
+    return(0);
+}
+
+int main()
+{
+    
+    PHY_PowerDown();
+    pc.baud(460800);
+//    pc.baud(9600);
+    right = 0.0;
+    left = 0.0;
+    flipper.attach(&flip, 0.2);
+   
+    // USB Init is done for Bluetooth
+    USBInit();
+    
+    while(1)
+    {
+        // USB Processing is done for Bluetooth
+        USBLoop();
+        
+    }
+}