Junichi Katsu / Mbed 2 deprecated WallbotR4

Test Ver

Dependencies:   mbed FatFileSystem

main.cpp@0:269589d8d2c2, 2012-11-17 (annotated)

Committer:
jksoft
Date:
Sat Nov 17 13:22:00 2012 +0000
Revision:
0:269589d8d2c2
Test Program

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jksoft 0:269589d8d2c2 1 /*
jksoft 0:269589d8d2c2 2 Copyright (c) 2012 JKSOFT
jksoft 0:269589d8d2c2 3
jksoft 0:269589d8d2c2 4 Permission is hereby granted, free of charge, to any person obtaining a copy
jksoft 0:269589d8d2c2 5 of this software and associated documentation files (the "Software"), to deal
jksoft 0:269589d8d2c2 6 in the Software without restriction, including without limitation the rights
jksoft 0:269589d8d2c2 7 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
jksoft 0:269589d8d2c2 8 copies of the Software, and to permit persons to whom the Software is
jksoft 0:269589d8d2c2 9 furnished to do so, subject to the following conditions:
jksoft 0:269589d8d2c2 10
jksoft 0:269589d8d2c2 11 The above copyright notice and this permission notice shall be included in
jksoft 0:269589d8d2c2 12 all copies or substantial portions of the Software.
jksoft 0:269589d8d2c2 13
jksoft 0:269589d8d2c2 14 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
jksoft 0:269589d8d2c2 15 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
jksoft 0:269589d8d2c2 16 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
jksoft 0:269589d8d2c2 17 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
jksoft 0:269589d8d2c2 18 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
jksoft 0:269589d8d2c2 19 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
jksoft 0:269589d8d2c2 20 THE SOFTWARE.
jksoft 0:269589d8d2c2 21 */
jksoft 0:269589d8d2c2 22
jksoft 0:269589d8d2c2 23 #include "mbed.h"
jksoft 0:269589d8d2c2 24 #include "USBHost.h"
jksoft 0:269589d8d2c2 25 #include "Utils.h"
jksoft 0:269589d8d2c2 26 #include "Wiimote.h"
jksoft 0:269589d8d2c2 27 #include "HighSpeedAnalogIn.h"
jksoft 0:269589d8d2c2 28 #include "EthernetPowerControl.h"
jksoft 0:269589d8d2c2 29 #include "TB6612.h"
jksoft 0:269589d8d2c2 30
jksoft 0:269589d8d2c2 31 #if 0
jksoft 0:269589d8d2c2 32 #define DBG(x) x
jksoft 0:269589d8d2c2 33 #else
jksoft 0:269589d8d2c2 34 #define DBG(x)
jksoft 0:269589d8d2c2 35 #endif
jksoft 0:269589d8d2c2 36
jksoft 0:269589d8d2c2 37 // PID terms
jksoft 0:269589d8d2c2 38 #define P_TERM 1
jksoft 0:269589d8d2c2 39 #define I_TERM 0
jksoft 0:269589d8d2c2 40 #define D_TERM 20
jksoft 0:269589d8d2c2 41
jksoft 0:269589d8d2c2 42 #define MAX 1.0
jksoft 0:269589d8d2c2 43 #define MIN -1.0
jksoft 0:269589d8d2c2 44
jksoft 0:269589d8d2c2 45 #define MAX_SPEED 100
jksoft 0:269589d8d2c2 46
jksoft 0:269589d8d2c2 47 Serial pc(USBTX, USBRX);
jksoft 0:269589d8d2c2 48 BusOut myleds(LED1, LED2, LED3, LED4);
jksoft 0:269589d8d2c2 49 BusOut myleds2(p7,p8,p9,p10);
jksoft 0:269589d8d2c2 50
jksoft 0:269589d8d2c2 51 // ----- Wallbot I/O Setting -----
jksoft 0:269589d8d2c2 52 // Motor
jksoft 0:269589d8d2c2 53 TB6612 right(p21,p12,p11);
jksoft 0:269589d8d2c2 54 TB6612 left(p22,p14,p13);
jksoft 0:269589d8d2c2 55
jksoft 0:269589d8d2c2 56 HighSpeedAnalogIn ain(p15, p16, p17, p18, p19, p20);
jksoft 0:269589d8d2c2 57
jksoft 0:269589d8d2c2 58 Ticker flipper;
jksoft 0:269589d8d2c2 59 int com_time_out = 0;
jksoft 0:269589d8d2c2 60 int com_stat = 0;
jksoft 0:269589d8d2c2 61 int move_time = 0;
jksoft 0:269589d8d2c2 62 int lmp = 0,kind = 0;
jksoft 0:269589d8d2c2 63
jksoft 0:269589d8d2c2 64 extern "C" void mbed_reset();
jksoft 0:269589d8d2c2 65
jksoft 0:269589d8d2c2 66 // p20 p19 p18 p17
jksoft 0:269589d8d2c2 67 // LEFT o o o o RIGHT
jksoft 0:269589d8d2c2 68 float GetSensor(int sh)
jksoft 0:269589d8d2c2 69 {
jksoft 0:269589d8d2c2 70 float ret = 0.0;
jksoft 0:269589d8d2c2 71 int bit = 0;
jksoft 0:269589d8d2c2 72 int value[4];
jksoft 0:269589d8d2c2 73
jksoft 0:269589d8d2c2 74 value[0] = ain.read_u16(p17);
jksoft 0:269589d8d2c2 75 value[1] = ain.read_u16(p18);
jksoft 0:269589d8d2c2 76 value[2] = ain.read_u16(p19);
jksoft 0:269589d8d2c2 77 value[3] = ain.read_u16(p20);
jksoft 0:269589d8d2c2 78
jksoft 0:269589d8d2c2 79 if( value[0] > sh ) bit |= 0x01;
jksoft 0:269589d8d2c2 80 if( value[1] > sh ) bit |= 0x02;
jksoft 0:269589d8d2c2 81 if( value[2] > sh ) bit |= 0x04;
jksoft 0:269589d8d2c2 82 if( value[3] > sh ) bit |= 0x08;
jksoft 0:269589d8d2c2 83
jksoft 0:269589d8d2c2 84 myleds = bit;
jksoft 0:269589d8d2c2 85
jksoft 0:269589d8d2c2 86 switch(bit)
jksoft 0:269589d8d2c2 87 {
jksoft 0:269589d8d2c2 88 case 0x01: ret = 1.0; break;
jksoft 0:269589d8d2c2 89 case 0x03: ret = 0.66; break;
jksoft 0:269589d8d2c2 90 case 0x02: ret = 0.33; break;
jksoft 0:269589d8d2c2 91 case 0x04: ret = -0.33; break;
jksoft 0:269589d8d2c2 92 case 0x0C: ret = -0.66; break;
jksoft 0:269589d8d2c2 93 case 0x08: ret = -1.0; break;
jksoft 0:269589d8d2c2 94 default: ret = 0.0; break;
jksoft 0:269589d8d2c2 95 }
jksoft 0:269589d8d2c2 96
jksoft 0:269589d8d2c2 97 // DBG(printf("[SENSOR] %d\t %d\t %d\t %d\t [%02X] : %f\n",value[0],value[1],value[2],value[3],bit,ret);)
jksoft 0:269589d8d2c2 98
jksoft 0:269589d8d2c2 99 return(ret);
jksoft 0:269589d8d2c2 100 }
jksoft 0:269589d8d2c2 101
jksoft 0:269589d8d2c2 102 float GetSensor(int sh, int *stat)
jksoft 0:269589d8d2c2 103 {
jksoft 0:269589d8d2c2 104 float ret = 0.0;
jksoft 0:269589d8d2c2 105 int bit = 0;
jksoft 0:269589d8d2c2 106 int value[4];
jksoft 0:269589d8d2c2 107
jksoft 0:269589d8d2c2 108 value[0] = ain.read_u16(p17);
jksoft 0:269589d8d2c2 109 value[1] = ain.read_u16(p18);
jksoft 0:269589d8d2c2 110 value[2] = ain.read_u16(p19);
jksoft 0:269589d8d2c2 111 value[3] = ain.read_u16(p20);
jksoft 0:269589d8d2c2 112
jksoft 0:269589d8d2c2 113 if( value[0] > sh ) bit |= 0x01;
jksoft 0:269589d8d2c2 114 if( value[1] > sh ) bit |= 0x02;
jksoft 0:269589d8d2c2 115 if( value[2] > sh ) bit |= 0x04;
jksoft 0:269589d8d2c2 116 if( value[3] > sh ) bit |= 0x08;
jksoft 0:269589d8d2c2 117
jksoft 0:269589d8d2c2 118 myleds = bit;
jksoft 0:269589d8d2c2 119 *stat = bit;
jksoft 0:269589d8d2c2 120
jksoft 0:269589d8d2c2 121 switch(bit)
jksoft 0:269589d8d2c2 122 {
jksoft 0:269589d8d2c2 123 case 0x01: ret = 1.0; break;
jksoft 0:269589d8d2c2 124 case 0x03: ret = 0.66; break;
jksoft 0:269589d8d2c2 125 case 0x02: ret = 0.33; break;
jksoft 0:269589d8d2c2 126 case 0x04: ret = -0.33; break;
jksoft 0:269589d8d2c2 127 case 0x0C: ret = -0.66; break;
jksoft 0:269589d8d2c2 128 case 0x08: ret = -1.0; break;
jksoft 0:269589d8d2c2 129 default: ret = 0.0; break;
jksoft 0:269589d8d2c2 130 }
jksoft 0:269589d8d2c2 131
jksoft 0:269589d8d2c2 132 // DBG(printf("[SENSOR] %d\t %d\t %d\t %d\t [%02X] : %f\n",value[0],value[1],value[2],value[3],bit,ret);)
jksoft 0:269589d8d2c2 133
jksoft 0:269589d8d2c2 134 return(ret);
jksoft 0:269589d8d2c2 135 }
jksoft 0:269589d8d2c2 136
jksoft 0:269589d8d2c2 137 void flip() {
jksoft 0:269589d8d2c2 138 com_time_out++;
jksoft 0:269589d8d2c2 139 static int led_count = 0;
jksoft 0:269589d8d2c2 140
jksoft 0:269589d8d2c2 141 if(com_stat == 1)
jksoft 0:269589d8d2c2 142 {
jksoft 0:269589d8d2c2 143 if(com_time_out > 2)
jksoft 0:269589d8d2c2 144 {
jksoft 0:269589d8d2c2 145 right = 0.0;
jksoft 0:269589d8d2c2 146 left = 0.0;
jksoft 0:269589d8d2c2 147 mbed_reset();
jksoft 0:269589d8d2c2 148 }
jksoft 0:269589d8d2c2 149 }
jksoft 0:269589d8d2c2 150 else
jksoft 0:269589d8d2c2 151 {
jksoft 0:269589d8d2c2 152 myleds = !myleds;
jksoft 0:269589d8d2c2 153 }
jksoft 0:269589d8d2c2 154 if(com_time_out > 150)
jksoft 0:269589d8d2c2 155 {
jksoft 0:269589d8d2c2 156 right = 0.0;
jksoft 0:269589d8d2c2 157 left = 0.0;
jksoft 0:269589d8d2c2 158 mbed_reset();
jksoft 0:269589d8d2c2 159 }
jksoft 0:269589d8d2c2 160 if(move_time != 0)
jksoft 0:269589d8d2c2 161 {
jksoft 0:269589d8d2c2 162 move_time--;
jksoft 0:269589d8d2c2 163 }
jksoft 0:269589d8d2c2 164
jksoft 0:269589d8d2c2 165 switch(led_count)
jksoft 0:269589d8d2c2 166 {
jksoft 0:269589d8d2c2 167 case 0:
jksoft 0:269589d8d2c2 168 myleds2 = 1;
jksoft 0:269589d8d2c2 169 led_count++;
jksoft 0:269589d8d2c2 170 break;
jksoft 0:269589d8d2c2 171 case 1:
jksoft 0:269589d8d2c2 172 myleds2 = 2;
jksoft 0:269589d8d2c2 173 led_count++;
jksoft 0:269589d8d2c2 174 break;
jksoft 0:269589d8d2c2 175 case 2:
jksoft 0:269589d8d2c2 176 myleds2 = 4;
jksoft 0:269589d8d2c2 177 led_count++;
jksoft 0:269589d8d2c2 178 break;
jksoft 0:269589d8d2c2 179 case 3:
jksoft 0:269589d8d2c2 180 myleds2 = 8;
jksoft 0:269589d8d2c2 181 led_count++;
jksoft 0:269589d8d2c2 182 break;
jksoft 0:269589d8d2c2 183 case 4:
jksoft 0:269589d8d2c2 184 myleds2 = 4;
jksoft 0:269589d8d2c2 185 led_count++;
jksoft 0:269589d8d2c2 186 break;
jksoft 0:269589d8d2c2 187 case 5:
jksoft 0:269589d8d2c2 188 myleds2 = 2;
jksoft 0:269589d8d2c2 189 led_count=0;
jksoft 0:269589d8d2c2 190 break;
jksoft 0:269589d8d2c2 191 }
jksoft 0:269589d8d2c2 192
jksoft 0:269589d8d2c2 193 }
jksoft 0:269589d8d2c2 194
jksoft 0:269589d8d2c2 195 int LineFollowMode()
jksoft 0:269589d8d2c2 196 {
jksoft 0:269589d8d2c2 197 float line_pos;
jksoft 0:269589d8d2c2 198 float derivative,proportional,power;
jksoft 0:269589d8d2c2 199 float speed =0.7;
jksoft 0:269589d8d2c2 200
jksoft 0:269589d8d2c2 201 float right_v;
jksoft 0:269589d8d2c2 202 float left_v;
jksoft 0:269589d8d2c2 203
jksoft 0:269589d8d2c2 204 static float old_line_pos = 0.0;
jksoft 0:269589d8d2c2 205 static float integral = 0.0;
jksoft 0:269589d8d2c2 206
jksoft 0:269589d8d2c2 207
jksoft 0:269589d8d2c2 208 line_pos = GetSensor(2500);
jksoft 0:269589d8d2c2 209
jksoft 0:269589d8d2c2 210 proportional = line_pos;
jksoft 0:269589d8d2c2 211 integral += line_pos;
jksoft 0:269589d8d2c2 212 derivative = line_pos - old_line_pos;
jksoft 0:269589d8d2c2 213 old_line_pos = line_pos;
jksoft 0:269589d8d2c2 214
jksoft 0:269589d8d2c2 215 power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
jksoft 0:269589d8d2c2 216
jksoft 0:269589d8d2c2 217 right_v = speed-power;
jksoft 0:269589d8d2c2 218 left_v = speed+power;
jksoft 0:269589d8d2c2 219
jksoft 0:269589d8d2c2 220 // limit checks
jksoft 0:269589d8d2c2 221 if (right_v < MIN)
jksoft 0:269589d8d2c2 222 right_v = MIN;
jksoft 0:269589d8d2c2 223 else if (right_v > MAX)
jksoft 0:269589d8d2c2 224 right_v = MAX;
jksoft 0:269589d8d2c2 225
jksoft 0:269589d8d2c2 226 if (left_v < MIN)
jksoft 0:269589d8d2c2 227 left_v = MIN;
jksoft 0:269589d8d2c2 228 else if (left_v > MAX)
jksoft 0:269589d8d2c2 229 left_v = MAX;
jksoft 0:269589d8d2c2 230
jksoft 0:269589d8d2c2 231 left = left_v;
jksoft 0:269589d8d2c2 232 right = right_v;
jksoft 0:269589d8d2c2 233
jksoft 0:269589d8d2c2 234 return(0);
jksoft 0:269589d8d2c2 235 }
jksoft 0:269589d8d2c2 236
jksoft 0:269589d8d2c2 237 // Direct control mode
jksoft 0:269589d8d2c2 238 int DirectMode( Wiimote* wii, int stat )
jksoft 0:269589d8d2c2 239 {
jksoft 0:269589d8d2c2 240 float line_pos;
jksoft 0:269589d8d2c2 241 int ret = stat;
jksoft 0:269589d8d2c2 242
jksoft 0:269589d8d2c2 243 if( move_time == 0 )
jksoft 0:269589d8d2c2 244 {
jksoft 0:269589d8d2c2 245 if( wii->left )
jksoft 0:269589d8d2c2 246 {
jksoft 0:269589d8d2c2 247 right = -MAX_SPEED;
jksoft 0:269589d8d2c2 248 left = MAX_SPEED;
jksoft 0:269589d8d2c2 249 }
jksoft 0:269589d8d2c2 250 else if( wii->right )
jksoft 0:269589d8d2c2 251 {
jksoft 0:269589d8d2c2 252 right = MAX_SPEED;
jksoft 0:269589d8d2c2 253 left = -MAX_SPEED;
jksoft 0:269589d8d2c2 254 }
jksoft 0:269589d8d2c2 255 else if( wii->up )
jksoft 0:269589d8d2c2 256 {
jksoft 0:269589d8d2c2 257 right = MAX_SPEED;
jksoft 0:269589d8d2c2 258 left = MAX_SPEED;
jksoft 0:269589d8d2c2 259 }
jksoft 0:269589d8d2c2 260 else if( wii->down )
jksoft 0:269589d8d2c2 261 {
jksoft 0:269589d8d2c2 262 right = -MAX_SPEED;
jksoft 0:269589d8d2c2 263 left = -MAX_SPEED;
jksoft 0:269589d8d2c2 264 }
jksoft 0:269589d8d2c2 265 else
jksoft 0:269589d8d2c2 266 {
jksoft 0:269589d8d2c2 267 right = 0;
jksoft 0:269589d8d2c2 268 left = 0;
jksoft 0:269589d8d2c2 269 }
jksoft 0:269589d8d2c2 270
jksoft 0:269589d8d2c2 271 float factor = wii->wheel * 1.5f;
jksoft 0:269589d8d2c2 272
jksoft 0:269589d8d2c2 273 if(factor > 100.0f ) factor = 100.0f;
jksoft 0:269589d8d2c2 274 if(factor < -100.0f ) factor = -100.0f;
jksoft 0:269589d8d2c2 275
jksoft 0:269589d8d2c2 276 printf("%f\t%f\r\n",wii->wheel,factor);
jksoft 0:269589d8d2c2 277
jksoft 0:269589d8d2c2 278 int left_factor = (int)((factor <= 0.0) ? 100.0 : 100.0 - factor);
jksoft 0:269589d8d2c2 279 int right_factor = (int)((factor >= 0.0) ? 100.0 : 100.0 - (-factor));
jksoft 0:269589d8d2c2 280
jksoft 0:269589d8d2c2 281 if( wii->one )
jksoft 0:269589d8d2c2 282 {
jksoft 0:269589d8d2c2 283 right = right_factor;
jksoft 0:269589d8d2c2 284 left = left_factor;
jksoft 0:269589d8d2c2 285 }
jksoft 0:269589d8d2c2 286 if( wii->two )
jksoft 0:269589d8d2c2 287 {
jksoft 0:269589d8d2c2 288 right = -left_factor;
jksoft 0:269589d8d2c2 289 left = -right_factor;
jksoft 0:269589d8d2c2 290 }
jksoft 0:269589d8d2c2 291
jksoft 0:269589d8d2c2 292 }
jksoft 0:269589d8d2c2 293
jksoft 0:269589d8d2c2 294 return(ret);
jksoft 0:269589d8d2c2 295 }
jksoft 0:269589d8d2c2 296
jksoft 0:269589d8d2c2 297 // Processing when receiving it from Wiiremote
jksoft 0:269589d8d2c2 298 int wall_bot_remote(char *c,int stat)
jksoft 0:269589d8d2c2 299 {
jksoft 0:269589d8d2c2 300 Wiimote wii;
jksoft 0:269589d8d2c2 301 int ret = stat;
jksoft 0:269589d8d2c2 302
jksoft 0:269589d8d2c2 303 wii.decode(c);
jksoft 0:269589d8d2c2 304
jksoft 0:269589d8d2c2 305 ret = DirectMode( &wii ,ret );
jksoft 0:269589d8d2c2 306
jksoft 0:269589d8d2c2 307 return(ret);
jksoft 0:269589d8d2c2 308 }
jksoft 0:269589d8d2c2 309
jksoft 0:269589d8d2c2 310 int GetConsoleChar()
jksoft 0:269589d8d2c2 311 {
jksoft 0:269589d8d2c2 312 return(0);
jksoft 0:269589d8d2c2 313 }
jksoft 0:269589d8d2c2 314
jksoft 0:269589d8d2c2 315 int OnDiskInsert(int device)
jksoft 0:269589d8d2c2 316 {
jksoft 0:269589d8d2c2 317 return(0);
jksoft 0:269589d8d2c2 318 }
jksoft 0:269589d8d2c2 319
jksoft 0:269589d8d2c2 320 int main()
jksoft 0:269589d8d2c2 321 {
jksoft 0:269589d8d2c2 322
jksoft 0:269589d8d2c2 323 PHY_PowerDown();
jksoft 0:269589d8d2c2 324 pc.baud(460800);
jksoft 0:269589d8d2c2 325 // pc.baud(9600);
jksoft 0:269589d8d2c2 326 right = 0.0;
jksoft 0:269589d8d2c2 327 left = 0.0;
jksoft 0:269589d8d2c2 328 flipper.attach(&flip, 0.2);
jksoft 0:269589d8d2c2 329
jksoft 0:269589d8d2c2 330 // USB Init is done for Bluetooth
jksoft 0:269589d8d2c2 331 USBInit();
jksoft 0:269589d8d2c2 332
jksoft 0:269589d8d2c2 333 while(1)
jksoft 0:269589d8d2c2 334 {
jksoft 0:269589d8d2c2 335 // USB Processing is done for Bluetooth
jksoft 0:269589d8d2c2 336 USBLoop();
jksoft 0:269589d8d2c2 337
jksoft 0:269589d8d2c2 338 }
jksoft 0:269589d8d2c2 339 }