WallBot_Simple - うおーるぼっと用プログラム Wiiリモコンからのダイレクト操作モードのみ BlueUSBをベースに…

Users » jksoft » Code » WallBot_Simple

Junichi Katsu / Mbed 2 deprecated WallBot_Simple

うおーるぼっと用プログラム Wiiリモコンからのダイレクト操作モードのみ BlueUSBをベースに使用しています。

Dependencies: BD6211F mbed SimpleFilter

main.cpp@0:4f749f62c6d7, 2011-04-29 (annotated)

Committer:: jksoft
Date:: Fri Apr 29 15:50:23 2011 +0000
Revision:: 0:4f749f62c6d7

Who changed what in which revision?

User	Revision	Line number	New contents of line
jksoft	0:4f749f62c6d7	1	/*
jksoft	0:4f749f62c6d7	2	Copyright (c) 2011 JKSOFT
jksoft	0:4f749f62c6d7	3
jksoft	0:4f749f62c6d7	4	Permission is hereby granted, free of charge, to any person obtaining a copy
jksoft	0:4f749f62c6d7	5	of this software and associated documentation files (the "Software"), to deal
jksoft	0:4f749f62c6d7	6	in the Software without restriction, including without limitation the rights
jksoft	0:4f749f62c6d7	7	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
jksoft	0:4f749f62c6d7	8	copies of the Software, and to permit persons to whom the Software is
jksoft	0:4f749f62c6d7	9	furnished to do so, subject to the following conditions:
jksoft	0:4f749f62c6d7	10
jksoft	0:4f749f62c6d7	11	The above copyright notice and this permission notice shall be included in
jksoft	0:4f749f62c6d7	12	all copies or substantial portions of the Software.
jksoft	0:4f749f62c6d7	13
jksoft	0:4f749f62c6d7	14	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
jksoft	0:4f749f62c6d7	15	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
jksoft	0:4f749f62c6d7	16	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
jksoft	0:4f749f62c6d7	17	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
jksoft	0:4f749f62c6d7	18	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
jksoft	0:4f749f62c6d7	19	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
jksoft	0:4f749f62c6d7	20	THE SOFTWARE.
jksoft	0:4f749f62c6d7	21	*/
jksoft	0:4f749f62c6d7	22
jksoft	0:4f749f62c6d7	23	#include "mbed.h"
jksoft	0:4f749f62c6d7	24	#include "USBHost.h"
jksoft	0:4f749f62c6d7	25	#include "Utils.h"
jksoft	0:4f749f62c6d7	26	#include "BD6211F.h"
jksoft	0:4f749f62c6d7	27	#include "Wiimote.h"
jksoft	0:4f749f62c6d7	28	#include "EthernetPowerControl.h"
jksoft	0:4f749f62c6d7	29	#include "SimpleFilter.h"
jksoft	0:4f749f62c6d7	30
jksoft	0:4f749f62c6d7	31	// #define _SENSOR_6
jksoft	0:4f749f62c6d7	32
jksoft	0:4f749f62c6d7	33	#define SENSOR_CYCLE 0.005f // 5ms
jksoft	0:4f749f62c6d7	34	#define SENSOR_SAMPLING 0.020f // 20ms
jksoft	0:4f749f62c6d7	35	#ifndef _SENSOR_6
jksoft	0:4f749f62c6d7	36	#define SENSOR_NUM 4
jksoft	0:4f749f62c6d7	37	#else
jksoft	0:4f749f62c6d7	38	#define SENSOR_NUM 6
jksoft	0:4f749f62c6d7	39	#endif
jksoft	0:4f749f62c6d7	40
jksoft	0:4f749f62c6d7	41	#define USB_INIT_CNT_MAX 40 // 0.5s X 40 = 20s
jksoft	0:4f749f62c6d7	42
jksoft	0:4f749f62c6d7	43	// ----- Wallbot I/O Setting -----
jksoft	0:4f749f62c6d7	44
jksoft	0:4f749f62c6d7	45	// Motor
jksoft	0:4f749f62c6d7	46	BD6211F RightMotor(p21,p22);
jksoft	0:4f749f62c6d7	47	BD6211F LeftMotor(p23,p24);
jksoft	0:4f749f62c6d7	48
jksoft	0:4f749f62c6d7	49	// Floor Sensor
jksoft	0:4f749f62c6d7	50	AnalogIn RightSideSensor(p15);
jksoft	0:4f749f62c6d7	51	AnalogIn RightCenterSensor(p16);
jksoft	0:4f749f62c6d7	52	AnalogIn LeftCenterSensor(p17);
jksoft	0:4f749f62c6d7	53	AnalogIn LeftSideSensor(p18);
jksoft	0:4f749f62c6d7	54	#ifdef _SENSOR_6
jksoft	0:4f749f62c6d7	55	AnalogIn RightBackSensor(p19);
jksoft	0:4f749f62c6d7	56	AnalogIn LeftBackSensor(p20);
jksoft	0:4f749f62c6d7	57	#endif
jksoft	0:4f749f62c6d7	58
jksoft	0:4f749f62c6d7	59	#ifndef _SENSOR_6
jksoft	0:4f749f62c6d7	60	AnalogIn _FloorSensor[] = {
jksoft	0:4f749f62c6d7	61	RightSideSensor ,
jksoft	0:4f749f62c6d7	62	RightCenterSensor ,
jksoft	0:4f749f62c6d7	63	LeftCenterSensor ,
jksoft	0:4f749f62c6d7	64	LeftSideSensor
jksoft	0:4f749f62c6d7	65	};
jksoft	0:4f749f62c6d7	66	#else
jksoft	0:4f749f62c6d7	67	AnalogIn _FloorSensor[] = {
jksoft	0:4f749f62c6d7	68	RightSideSensor ,
jksoft	0:4f749f62c6d7	69	RightCenterSensor ,
jksoft	0:4f749f62c6d7	70	LeftCenterSensor ,
jksoft	0:4f749f62c6d7	71	LeftSideSensor ,
jksoft	0:4f749f62c6d7	72	RightBackSensor ,
jksoft	0:4f749f62c6d7	73	LeftBackSensor
jksoft	0:4f749f62c6d7	74	};
jksoft	0:4f749f62c6d7	75	#endif
jksoft	0:4f749f62c6d7	76	// Setting Sw
jksoft	0:4f749f62c6d7	77	// It is a Pull-Up
jksoft	0:4f749f62c6d7	78	DigitalIn sw1(p29);
jksoft	0:4f749f62c6d7	79	DigitalIn sw2(p30);
jksoft	0:4f749f62c6d7	80
jksoft	0:4f749f62c6d7	81	// LED
jksoft	0:4f749f62c6d7	82	DigitalOut chk_led(LED4);
jksoft	0:4f749f62c6d7	83
jksoft	0:4f749f62c6d7	84	// -------------------------------
jksoft	0:4f749f62c6d7	85
jksoft	0:4f749f62c6d7	86	// Input processing (Sensor & Sw)
jksoft	0:4f749f62c6d7	87	Ticker sensor;
jksoft	0:4f749f62c6d7	88	int sampling = (int)(SENSOR_SAMPLING / SENSOR_CYCLE);
jksoft	0:4f749f62c6d7	89	int sw[2][10];
jksoft	0:4f749f62c6d7	90
jksoft	0:4f749f62c6d7	91	SimpleFilter fl1(4);
jksoft	0:4f749f62c6d7	92	SimpleFilter fl2(4);
jksoft	0:4f749f62c6d7	93	SimpleFilter fl3(4);
jksoft	0:4f749f62c6d7	94	SimpleFilter fl4(4);
jksoft	0:4f749f62c6d7	95	SimpleFilter fl5(4);
jksoft	0:4f749f62c6d7	96	SimpleFilter fl6(4);
jksoft	0:4f749f62c6d7	97	SimpleFilter FloorSensor_fl[] = { fl1, fl2, fl3, fl4, fl5, fl6 };
jksoft	0:4f749f62c6d7	98
jksoft	0:4f749f62c6d7	99	// Wii remote chk
jksoft	0:4f749f62c6d7	100	int wd_timer = 0;
jksoft	0:4f749f62c6d7	101	int usb_init_count = 0;
jksoft	0:4f749f62c6d7	102
jksoft	0:4f749f62c6d7	103	// Timer tick(SENSOR_CYCLE[ms])
jksoft	0:4f749f62c6d7	104	void cycle_proc()
jksoft	0:4f749f62c6d7	105	{
jksoft	0:4f749f62c6d7	106	static int counter = 0;
jksoft	0:4f749f62c6d7	107	int i;
jksoft	0:4f749f62c6d7	108
jksoft	0:4f749f62c6d7	109	if( counter >= sampling )
jksoft	0:4f749f62c6d7	110	{
jksoft	0:4f749f62c6d7	111	counter = 0;
jksoft	0:4f749f62c6d7	112	}
jksoft	0:4f749f62c6d7	113
jksoft	0:4f749f62c6d7	114	// Sw
jksoft	0:4f749f62c6d7	115	sw[0][counter] = sw1;
jksoft	0:4f749f62c6d7	116	sw[1][counter] = sw2;
jksoft	0:4f749f62c6d7	117
jksoft	0:4f749f62c6d7	118
jksoft	0:4f749f62c6d7	119	// FloorSensor
jksoft	0:4f749f62c6d7	120	for( i = 0 ; i < SENSOR_NUM ; i++ )
jksoft	0:4f749f62c6d7	121	{
jksoft	0:4f749f62c6d7	122	FloorSensor_fl[i].filter(_FloorSensor[i].read_u16() >> 1);
jksoft	0:4f749f62c6d7	123	}
jksoft	0:4f749f62c6d7	124
jksoft	0:4f749f62c6d7	125
jksoft	0:4f749f62c6d7	126	counter ++;
jksoft	0:4f749f62c6d7	127	wd_timer ++;
jksoft	0:4f749f62c6d7	128	}
jksoft	0:4f749f62c6d7	129
jksoft	0:4f749f62c6d7	130	//
jksoft	0:4f749f62c6d7	131	int GetFloorSensor(int num)
jksoft	0:4f749f62c6d7	132	{
jksoft	0:4f749f62c6d7	133	int ret;
jksoft	0:4f749f62c6d7	134
jksoft	0:4f749f62c6d7	135	if( num >= SENSOR_NUM ) return( 0 );
jksoft	0:4f749f62c6d7	136
jksoft	0:4f749f62c6d7	137	ret = (int)FloorSensor_fl[num].value();
jksoft	0:4f749f62c6d7	138
jksoft	0:4f749f62c6d7	139	return(ret);
jksoft	0:4f749f62c6d7	140	}
jksoft	0:4f749f62c6d7	141
jksoft	0:4f749f62c6d7	142	// Sw ON:true OFF:false
jksoft	0:4f749f62c6d7	143	bool GetSw(int num)
jksoft	0:4f749f62c6d7	144	{
jksoft	0:4f749f62c6d7	145	int i,sum = 0;
jksoft	0:4f749f62c6d7	146	bool ret = false;
jksoft	0:4f749f62c6d7	147
jksoft	0:4f749f62c6d7	148	for( i = 0 ; i < sampling ; i++ )
jksoft	0:4f749f62c6d7	149	{
jksoft	0:4f749f62c6d7	150	sum += sw[num][i];
jksoft	0:4f749f62c6d7	151	}
jksoft	0:4f749f62c6d7	152	if( sum == 0 )
jksoft	0:4f749f62c6d7	153	{
jksoft	0:4f749f62c6d7	154	ret = true;
jksoft	0:4f749f62c6d7	155	}
jksoft	0:4f749f62c6d7	156
jksoft	0:4f749f62c6d7	157	return( ret );
jksoft	0:4f749f62c6d7	158	}
jksoft	0:4f749f62c6d7	159
jksoft	0:4f749f62c6d7	160	// Direct control mode
jksoft	0:4f749f62c6d7	161	int DirectMode( Wiimote* wii, int stat )
jksoft	0:4f749f62c6d7	162	{
jksoft	0:4f749f62c6d7	163	int ret = stat;
jksoft	0:4f749f62c6d7	164
jksoft	0:4f749f62c6d7	165	if( wii->left )
jksoft	0:4f749f62c6d7	166	{
jksoft	0:4f749f62c6d7	167	RightMotor = 1.0;
jksoft	0:4f749f62c6d7	168	LeftMotor = -1.0;
jksoft	0:4f749f62c6d7	169	}
jksoft	0:4f749f62c6d7	170	else if( wii->right )
jksoft	0:4f749f62c6d7	171	{
jksoft	0:4f749f62c6d7	172	RightMotor = -1.0;
jksoft	0:4f749f62c6d7	173	LeftMotor = 1.0;
jksoft	0:4f749f62c6d7	174	}
jksoft	0:4f749f62c6d7	175	else if( wii->up )
jksoft	0:4f749f62c6d7	176	{
jksoft	0:4f749f62c6d7	177	RightMotor = 1.0;
jksoft	0:4f749f62c6d7	178	LeftMotor = 1.0;
jksoft	0:4f749f62c6d7	179	}
jksoft	0:4f749f62c6d7	180	else if( wii->down )
jksoft	0:4f749f62c6d7	181	{
jksoft	0:4f749f62c6d7	182	RightMotor = -1.0;
jksoft	0:4f749f62c6d7	183	LeftMotor = -1.0;
jksoft	0:4f749f62c6d7	184	}
jksoft	0:4f749f62c6d7	185	else
jksoft	0:4f749f62c6d7	186	{
jksoft	0:4f749f62c6d7	187	RightMotor = 0.0;
jksoft	0:4f749f62c6d7	188	LeftMotor = 0.0;
jksoft	0:4f749f62c6d7	189	}
jksoft	0:4f749f62c6d7	190
jksoft	0:4f749f62c6d7	191	float factor = wii->wheel / 150.0f;
jksoft	0:4f749f62c6d7	192
jksoft	0:4f749f62c6d7	193	float left_factor = (factor >= 0.0) ? 1.0 : 1.0 - (-factor);
jksoft	0:4f749f62c6d7	194	float right_factor = (factor <= 0.0) ? 1.0 : 1.0 - factor;
jksoft	0:4f749f62c6d7	195
jksoft	0:4f749f62c6d7	196	if( wii->one )
jksoft	0:4f749f62c6d7	197	{
jksoft	0:4f749f62c6d7	198	RightMotor = right_factor;
jksoft	0:4f749f62c6d7	199	LeftMotor = left_factor;
jksoft	0:4f749f62c6d7	200	}
jksoft	0:4f749f62c6d7	201	if( wii->two )
jksoft	0:4f749f62c6d7	202	{
jksoft	0:4f749f62c6d7	203	RightMotor = -left_factor;
jksoft	0:4f749f62c6d7	204	LeftMotor = -right_factor;
jksoft	0:4f749f62c6d7	205	}
jksoft	0:4f749f62c6d7	206
jksoft	0:4f749f62c6d7	207	return(ret);
jksoft	0:4f749f62c6d7	208	}
jksoft	0:4f749f62c6d7	209
jksoft	0:4f749f62c6d7	210	// Processing when receiving it from Wiiremote
jksoft	0:4f749f62c6d7	211	int wall_bot_remote(char *c,int stat)
jksoft	0:4f749f62c6d7	212	{
jksoft	0:4f749f62c6d7	213	Wiimote wii;
jksoft	0:4f749f62c6d7	214	int ret = stat;
jksoft	0:4f749f62c6d7	215
jksoft	0:4f749f62c6d7	216	wii.decode(c);
jksoft	0:4f749f62c6d7	217
jksoft	0:4f749f62c6d7	218	ret = DirectMode( &wii ,ret );
jksoft	0:4f749f62c6d7	219
jksoft	0:4f749f62c6d7	220	wd_timer = 0;
jksoft	0:4f749f62c6d7	221	chk_led = 0;
jksoft	0:4f749f62c6d7	222	usb_init_count = USB_INIT_CNT_MAX;
jksoft	0:4f749f62c6d7	223
jksoft	0:4f749f62c6d7	224	return(ret);
jksoft	0:4f749f62c6d7	225	}
jksoft	0:4f749f62c6d7	226
jksoft	0:4f749f62c6d7	227	void input_chk(void)
jksoft	0:4f749f62c6d7	228	{
jksoft	0:4f749f62c6d7	229	short fl[4];
jksoft	0:4f749f62c6d7	230	int i;
jksoft	0:4f749f62c6d7	231	bool sw[2];
jksoft	0:4f749f62c6d7	232
jksoft	0:4f749f62c6d7	233	for(i=0;i<4;i++)
jksoft	0:4f749f62c6d7	234	{
jksoft	0:4f749f62c6d7	235	fl[i] = GetFloorSensor(i);
jksoft	0:4f749f62c6d7	236	}
jksoft	0:4f749f62c6d7	237	sw[0] = GetSw(0);
jksoft	0:4f749f62c6d7	238	sw[1] = GetSw(1);
jksoft	0:4f749f62c6d7	239
jksoft	0:4f749f62c6d7	240	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]);
jksoft	0:4f749f62c6d7	241
jksoft	0:4f749f62c6d7	242	}
jksoft	0:4f749f62c6d7	243
jksoft	0:4f749f62c6d7	244	void output_chk(void)
jksoft	0:4f749f62c6d7	245	{
jksoft	0:4f749f62c6d7	246	static int step = 0;
jksoft	0:4f749f62c6d7	247
jksoft	0:4f749f62c6d7	248	switch(step)
jksoft	0:4f749f62c6d7	249	{
jksoft	0:4f749f62c6d7	250	case 0:
jksoft	0:4f749f62c6d7	251	RightMotor = 1.0;
jksoft	0:4f749f62c6d7	252	LeftMotor = 1.0;
jksoft	0:4f749f62c6d7	253	break;
jksoft	0:4f749f62c6d7	254	case 1:
jksoft	0:4f749f62c6d7	255	RightMotor = -1.0;
jksoft	0:4f749f62c6d7	256	LeftMotor = -1.0;
jksoft	0:4f749f62c6d7	257	break;
jksoft	0:4f749f62c6d7	258	case 2:
jksoft	0:4f749f62c6d7	259	RightMotor = -1.0;
jksoft	0:4f749f62c6d7	260	LeftMotor = 1.0;
jksoft	0:4f749f62c6d7	261	break;
jksoft	0:4f749f62c6d7	262	case 3:
jksoft	0:4f749f62c6d7	263	RightMotor = 1.0;
jksoft	0:4f749f62c6d7	264	LeftMotor = -1.0;
jksoft	0:4f749f62c6d7	265	break;
jksoft	0:4f749f62c6d7	266	}
jksoft	0:4f749f62c6d7	267	step = (step+1)%4;
jksoft	0:4f749f62c6d7	268	}
jksoft	0:4f749f62c6d7	269
jksoft	0:4f749f62c6d7	270	// Wii Time Out chk
jksoft	0:4f749f62c6d7	271	void wd_wii_chk(void)
jksoft	0:4f749f62c6d7	272	{
jksoft	0:4f749f62c6d7	273	if( wd_timer > (0.5/SENSOR_CYCLE) )
jksoft	0:4f749f62c6d7	274	{
jksoft	0:4f749f62c6d7	275	chk_led = !chk_led;
jksoft	0:4f749f62c6d7	276
jksoft	0:4f749f62c6d7	277	wd_timer = 0;
jksoft	0:4f749f62c6d7	278
jksoft	0:4f749f62c6d7	279	RightMotor.speed(0.0);
jksoft	0:4f749f62c6d7	280	LeftMotor.speed(0.0);
jksoft	0:4f749f62c6d7	281
jksoft	0:4f749f62c6d7	282	usb_init_count++;
jksoft	0:4f749f62c6d7	283
jksoft	0:4f749f62c6d7	284	if(usb_init_count > USB_INIT_CNT_MAX)
jksoft	0:4f749f62c6d7	285	{
jksoft	0:4f749f62c6d7	286	USBInit();
jksoft	0:4f749f62c6d7	287	usb_init_count = 0;
jksoft	0:4f749f62c6d7	288	}
jksoft	0:4f749f62c6d7	289	}
jksoft	0:4f749f62c6d7	290	}
jksoft	0:4f749f62c6d7	291
jksoft	0:4f749f62c6d7	292	int GetConsoleChar()
jksoft	0:4f749f62c6d7	293	{
jksoft	0:4f749f62c6d7	294	return(0);
jksoft	0:4f749f62c6d7	295	}
jksoft	0:4f749f62c6d7	296
jksoft	0:4f749f62c6d7	297	int OnDiskInsert(int device)
jksoft	0:4f749f62c6d7	298	{
jksoft	0:4f749f62c6d7	299	return(0);
jksoft	0:4f749f62c6d7	300	}
jksoft	0:4f749f62c6d7	301
jksoft	0:4f749f62c6d7	302	int main()
jksoft	0:4f749f62c6d7	303	{
jksoft	0:4f749f62c6d7	304	// Init
jksoft	0:4f749f62c6d7	305
jksoft	0:4f749f62c6d7	306	// Ether PHY Stop
jksoft	0:4f749f62c6d7	307	PHY_PowerDown();
jksoft	0:4f749f62c6d7	308
jksoft	0:4f749f62c6d7	309	// Sw Pull up
jksoft	0:4f749f62c6d7	310	sw1.mode(PullUp);
jksoft	0:4f749f62c6d7	311	sw2.mode(PullUp);
jksoft	0:4f749f62c6d7	312
jksoft	0:4f749f62c6d7	313	// Motor stop
jksoft	0:4f749f62c6d7	314	RightMotor.speed(0.0);
jksoft	0:4f749f62c6d7	315	LeftMotor.speed(0.0);
jksoft	0:4f749f62c6d7	316
jksoft	0:4f749f62c6d7	317	// USB Init is done for Bluetooth
jksoft	0:4f749f62c6d7	318	USBInit();
jksoft	0:4f749f62c6d7	319	usb_init_count = 0;
jksoft	0:4f749f62c6d7	320
jksoft	0:4f749f62c6d7	321	// Sensing Processing
jksoft	0:4f749f62c6d7	322	sensor.attach(&cycle_proc, SENSOR_CYCLE);
jksoft	0:4f749f62c6d7	323
jksoft	0:4f749f62c6d7	324	while(1)
jksoft	0:4f749f62c6d7	325	{
jksoft	0:4f749f62c6d7	326
jksoft	0:4f749f62c6d7	327	// USB Processing is done for Bluetooth
jksoft	0:4f749f62c6d7	328	USBLoop();
jksoft	0:4f749f62c6d7	329	wd_wii_chk();
jksoft	0:4f749f62c6d7	330
jksoft	0:4f749f62c6d7	331	// input_chk();
jksoft	0:4f749f62c6d7	332	// output_chk();
jksoft	0:4f749f62c6d7	333	// wait(0.1);
jksoft	0:4f749f62c6d7	334	}
jksoft	0:4f749f62c6d7	335	}

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	29 Apr 2011
Imports:	42
Forks:	0
Commits:	1
Dependents:	0
Dependencies:	3
Followers:	2

main.cpp@0:4f749f62c6d7, 2011-04-29 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning