Test Ver
Dependencies: mbed FatFileSystem
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(); + + } +}