Catchtest - 腕のプログラムです.

Bチーム » Code » Catchtest

腕のプログラムです.

Dependencies: mbed TextLCD MultiSerial Pswitch

main.cpp@8:4f9c4b791bfe, 2014-10-05 (annotated)

Committer:: lilac0112_1
Date:: Sun Oct 05 08:21:56 2014 +0000
Revision:: 8:4f9c4b791bfe
Parent:: 7:c418a0d929b1
Child:: 9:8543cb951a70

Who changed what in which revision?

User	Revision	Line number	New contents of line
lilac0112_1	0:4baeb4381394	1	#include "mbed.h"
Hatter	3:a1489a94c436	2	#include "MultiSerial.h"
lilac0112_1	7:c418a0d929b1	3	#include "Pswitch.h"
lilac0112_1	7:c418a0d929b1	4
lilac0112_1	4:1102ab499430	5	#define DATA_NUM 1 //DATA_NUM[byte]通信
lilac0112_1	7:c418a0d929b1	6	#define KEYCODE 0xAA
lilac0112_1	7:c418a0d929b1	7
lilac0112_1	7:c418a0d929b1	8	#define TIME_LIMIT 1.25//腕が閉まっても動けるのはTIME_LIMIT[s]まで
lilac0112_1	4:1102ab499430	9	#define ARM_REV 1.0//腕のPWM
lilac0112_1	7:c418a0d929b1	10
lilac0112_1	8:4f9c4b791bfe	11	#define TURN_REV 0.2//回転の初期PWM値
lilac0112_1	1:006debaed874	12
lilac0112_1	7:c418a0d929b1	13
lilac0112_1	4:1102ab499430	14	uint8_t INdata[DATA_NUM]={0}, EXdata[DATA_NUM]={0};
lilac0112_1	7:c418a0d929b1	15	uint8_t Interrupt=0;
lilac0112_1	4:1102ab499430	16	uint8_t data[DATA_NUM];
lilac0112_1	1:006debaed874	17
lilac0112_1	1:006debaed874	18	enum Logic{OFF=0, ON};//殆どはモーターのON，OFFに使う
lilac0112_1	1:006debaed874	19	enum Arm{R=0, L, C};//右腕と左腕
lilac0112_1	1:006debaed874	20	enum Clip{Open=0, Close};//開ける動作と閉める動作
lilac0112_1	2:eb1c123c9140	21	enum Revolution{CW=0, CCW};//正回転，逆回転(上体回転)
lilac0112_1	2:eb1c123c9140	22	enum Compass{N=0, E, W, S};//マシンの回転
lilac0112_1	7:c418a0d929b1	23
lilac0112_1	0:4baeb4381394	24	BusOut LED(LED1, LED2, LED3, LED4);//確認用
lilac0112_1	7:c418a0d929b1	25
Hatter	3:a1489a94c436	26	/*
Hatter	3:a1489a94c436	27	BusIn mt(p19, p20, p17, p18);//Switch
Hatter	3:a1489a94c436	28	*/
lilac0112_1	7:c418a0d929b1	29
lilac0112_1	7:c418a0d929b1	30
lilac0112_1	7:c418a0d929b1	31	BusIn mt(p5, p6, p7, p8);//mt(p7, p8);
lilac0112_1	5:c2a278842131	32	DigitalIn OneEighty[2]={p18, p19};//R, L
lilac0112_1	7:c418a0d929b1	33	Pswitch HandSW[3]={p15, p16, p17};//p14, p15, p16//R, L, C
lilac0112_1	2:eb1c123c9140	34	DigitalIn TurnSW[2]={p11, p12};//
lilac0112_1	2:eb1c123c9140	35	PwmOut HandSpeed[2]={p23, p24};
lilac0112_1	2:eb1c123c9140	36	PwmOut TurnSpeed(p22);//p22
lilac0112_1	2:eb1c123c9140	37	DigitalOut Hand[2][2]={{p28, p27}, {p26, p25}};//{p17, p18}, {p19, p20}
lilac0112_1	0:4baeb4381394	38	//ex)Hand[R][Close]=ON;
lilac0112_1	2:eb1c123c9140	39	DigitalOut Turn[2]={p29, p30};//p29, p30
lilac0112_1	2:eb1c123c9140	40	//ex)Turn[CW]=ON;
lilac0112_1	1:006debaed874	41	Serial pc(USBTX, USBRX);
lilac0112_1	0:4baeb4381394	42	Timeout Touch;//腕で掴むときの安全対策
lilac0112_1	1:006debaed874	43
lilac0112_1	1:006debaed874	44	void StopCatching(void);//literally
lilac0112_1	1:006debaed874	45	void StopLeaving(void);//literally
lilac0112_1	1:006debaed874	46	void StartCatching(void);//literally
lilac0112_1	1:006debaed874	47	void StartLeaving(void);//literally
lilac0112_1	1:006debaed874	48	void RightCatch(void);//右腕のリミットスイッチ
lilac0112_1	1:006debaed874	49	void LeftCatch(void);//左腕のリミットスイッチ
lilac0112_1	1:006debaed874	50	void CenterCatch(void);//中央のリミットスイッチ
lilac0112_1	4:1102ab499430	51	uint8_t LinkBit(bool eight, bool seven, bool six, bool five, bool fore, bool three, bool two, bool one);
lilac0112_1	4:1102ab499430	52	bool GetBit(uint8_t n, uint8_t bit);
lilac0112_1	1:006debaed874	53	void SetUp(void);//literally
lilac0112_1	7:c418a0d929b1	54
lilac0112_1	7:c418a0d929b1	55
lilac0112_1	1:006debaed874	56	int main(void) {
lilac0112_1	1:006debaed874	57
lilac0112_1	5:c2a278842131	58	wait(0.5);
lilac0112_1	5:c2a278842131	59
lilac0112_1	5:c2a278842131	60	MultiSerial leg_mbed(p13,p14,read);
lilac0112_1	7:c418a0d929b1	61	//Serial servo_mbed(p9, p10);
lilac0112_1	4:1102ab499430	62
lilac0112_1	1:006debaed874	63	SetUp();
lilac0112_1	1:006debaed874	64
lilac0112_1	1:006debaed874	65	while(1) {
lilac0112_1	1:006debaed874	66
Hatter	3:a1489a94c436	67	leg_mbed.read_data(data,KEYCODE);
Hatter	3:a1489a94c436	68
lilac0112_1	7:c418a0d929b1	69	INdata[0] = data[0];// \| ~mt;
lilac0112_1	7:c418a0d929b1	70
lilac0112_1	7:c418a0d929b1	71	//INdata[0] = ~mt;
lilac0112_1	7:c418a0d929b1	72
lilac0112_1	7:c418a0d929b1	73	//EXdata[0] = LinkBit(0, 0, 0, 0, 0, 0, !OneEighty[R], !OneEighty[L]);
lilac0112_1	7:c418a0d929b1	74	//EXdata[0] = 3;//~mt;
lilac0112_1	7:c418a0d929b1	75	LED = INdata[0];//~mt;//~mt;//EXdata[0]\| servo_mbed.getc()*4;
lilac0112_1	7:c418a0d929b1	76
lilac0112_1	7:c418a0d929b1	77	//servo_mbed.putc(~mt);
lilac0112_1	7:c418a0d929b1	78	//pc.printf("%5d, %f\n", Holder, TurnSpeed.read());
lilac0112_1	7:c418a0d929b1	79
lilac0112_1	7:c418a0d929b1	80	if((GetBit(INdata[0], 1)==OFF)&&(Interrupt==OFF)) StopCatching();
lilac0112_1	1:006debaed874	81
lilac0112_1	4:1102ab499430	82	if(GetBit(INdata[0], 2)==OFF) StopLeaving();//停止
lilac0112_1	1:006debaed874	83
lilac0112_1	4:1102ab499430	84	if((GetBit(INdata[0], 1)+GetBit(INdata[0], 2))==1){//同時押し防止
lilac0112_1	7:c418a0d929b1	85	if(GetBit(INdata[0], 1)==true) CenterCatch();//中央のリミットスイッチがうまく動作しなかった場合の保険
lilac0112_1	4:1102ab499430	86	if(GetBit(INdata[0], 2)==true){//腕を広げる動作はマニュアル操作
lilac0112_1	1:006debaed874	87	StopCatching();//停止
lilac0112_1	1:006debaed874	88	Touch.detach();//ワンショットタイマー停止
lilac0112_1	7:c418a0d929b1	89	Interrupt=OFF;
lilac0112_1	4:1102ab499430	90	Hand[R][Open] = GetBit(INdata[0], 2)&&OneEighty[R];
lilac0112_1	4:1102ab499430	91	Hand[L][Open] = GetBit(INdata[0], 2)&&OneEighty[L];
lilac0112_1	1:006debaed874	92	}
lilac0112_1	1:006debaed874	93	}
lilac0112_1	1:006debaed874	94
lilac0112_1	1:006debaed874	95	if(((!HandSW[R])&&(!HandSW[L]))&&((Hand[R][Close]&&Hand[L][Close])==ON)){//どちらも押されたら停止
lilac0112_1	1:006debaed874	96	StopCatching();//停止
lilac0112_1	1:006debaed874	97	Touch.detach();//ワンショットタイマー停止
lilac0112_1	7:c418a0d929b1	98	Interrupt=OFF;
lilac0112_1	1:006debaed874	99	}
lilac0112_1	1:006debaed874	100
lilac0112_1	2:eb1c123c9140	101	if(!OneEighty[R]) Hand[R][Open]=OFF;
lilac0112_1	2:eb1c123c9140	102	if(!OneEighty[L]) Hand[L][Open]=OFF;
lilac0112_1	7:c418a0d929b1	103
lilac0112_1	7:c418a0d929b1	104	if(!HandSW[R]) Hand[R][Close]=OFF;
lilac0112_1	7:c418a0d929b1	105	if(!HandSW[L]) Hand[L][Close]=OFF;
lilac0112_1	2:eb1c123c9140	106
lilac0112_1	7:c418a0d929b1	107	if(HandSW[R].count() > 0){
lilac0112_1	7:c418a0d929b1	108	RightCatch();
lilac0112_1	7:c418a0d929b1	109	}
lilac0112_1	7:c418a0d929b1	110	if(HandSW[L].count() > 0){
lilac0112_1	7:c418a0d929b1	111	LeftCatch();
lilac0112_1	7:c418a0d929b1	112	}
lilac0112_1	7:c418a0d929b1	113	if(HandSW[C].count() > 0){
lilac0112_1	7:c418a0d929b1	114	CenterCatch();
lilac0112_1	7:c418a0d929b1	115	}
lilac0112_1	2:eb1c123c9140	116
lilac0112_1	4:1102ab499430	117	//LED = ((0x01 * !HandSW[R]) \| (0x02 * !HandSW[L]) \| (0x04 * !HandSW[C]) \| (0x08 * ((!OneEighty[R])&&(!OneEighty[L])) ));
lilac0112_1	4:1102ab499430	118	LED = INdata[0];
lilac0112_1	2:eb1c123c9140	119
lilac0112_1	2:eb1c123c9140	120	//上体回転
lilac0112_1	2:eb1c123c9140	121
lilac0112_1	8:4f9c4b791bfe	122	if(GetBit(INdata[0], 3)==OFF) Turn[CW] = OFF;//停止
lilac0112_1	8:4f9c4b791bfe	123	if(GetBit(INdata[0], 4)==OFF) Turn[CCW] = OFF;//停止
lilac0112_1	7:c418a0d929b1	124
lilac0112_1	8:4f9c4b791bfe	125	if((GetBit(INdata[0], 3)+GetBit(INdata[0], 4))==1){//同時押し防止
lilac0112_1	4:1102ab499430	126
lilac0112_1	4:1102ab499430	127	Turn[CW] = GetBit(INdata[0], 3)&&TurnSW[N];//押されていて，かつリミットスイッチが押されていなかったら右回転
lilac0112_1	4:1102ab499430	128	Turn[CCW] = GetBit(INdata[0], 4)&&TurnSW[E];//押されていて，かつリミットスイッチが押されていなかったら左回転
lilac0112_1	2:eb1c123c9140	129
lilac0112_1	2:eb1c123c9140	130	}
lilac0112_1	5:c2a278842131	131
lilac0112_1	4:1102ab499430	132	if(!TurnSW[N]) Turn[CW]=OFF;//リミットスイッチによる停止
lilac0112_1	4:1102ab499430	133	if(!TurnSW[E]) Turn[CCW]=OFF;
lilac0112_1	4:1102ab499430	134
lilac0112_1	4:1102ab499430	135
lilac0112_1	1:006debaed874	136	}
lilac0112_1	1:006debaed874	137	}
lilac0112_1	0:4baeb4381394	138	void StopCatching(void){//停止
lilac0112_1	0:4baeb4381394	139	Hand[R][Close]=Hand[L][Close]=OFF;
lilac0112_1	7:c418a0d929b1	140	if(Interrupt==ON) Interrupt=OFF;
lilac0112_1	0:4baeb4381394	141	}
lilac0112_1	0:4baeb4381394	142	void StopLeaving(void){//停止
lilac0112_1	0:4baeb4381394	143	Hand[R][Open]=Hand[L][Open]=OFF;
lilac0112_1	0:4baeb4381394	144	}
lilac0112_1	2:eb1c123c9140	145	void StartCatching(void){//掴む（リミットスイッチが押されてなければ）
lilac0112_1	7:c418a0d929b1	146
lilac0112_1	7:c418a0d929b1	147	if(GetBit(INdata[0], 1)==true){
lilac0112_1	7:c418a0d929b1	148	Hand[R][Close]=HandSW[R];
lilac0112_1	7:c418a0d929b1	149	Hand[L][Close]=HandSW[L];
lilac0112_1	7:c418a0d929b1	150	}
lilac0112_1	7:c418a0d929b1	151
lilac0112_1	7:c418a0d929b1	152	if((!HandSW[R])&&(!HandSW[L])){
lilac0112_1	2:eb1c123c9140	153	Hand[R][Close] = Hand[L][Close] = OFF;
lilac0112_1	2:eb1c123c9140	154	}
lilac0112_1	2:eb1c123c9140	155	else{
lilac0112_1	2:eb1c123c9140	156
lilac0112_1	2:eb1c123c9140	157	Hand[R][Close]=ON;
lilac0112_1	2:eb1c123c9140	158	Hand[L][Close]=ON;
lilac0112_1	2:eb1c123c9140	159
lilac0112_1	2:eb1c123c9140	160	}
lilac0112_1	2:eb1c123c9140	161
lilac0112_1	0:4baeb4381394	162	}
lilac0112_1	2:eb1c123c9140	163	void StartLeaving(void){//離す（リミットスイッチが押されてなければ）
lilac0112_1	2:eb1c123c9140	164	Hand[R][Open]=OneEighty[R];
lilac0112_1	2:eb1c123c9140	165	Hand[L][Open]=OneEighty[L];
lilac0112_1	0:4baeb4381394	166	}
lilac0112_1	0:4baeb4381394	167	void RightCatch(void){//右腕のリミットスイッチ
lilac0112_1	7:c418a0d929b1	168
lilac0112_1	7:c418a0d929b1	169	Hand[R][Close] = OFF;
lilac0112_1	7:c418a0d929b1	170
lilac0112_1	0:4baeb4381394	171	if((!HandSW[R])&&(!HandSW[L])){//どちらも押されたら停止
lilac0112_1	0:4baeb4381394	172	StopCatching();//停止
lilac0112_1	0:4baeb4381394	173	Touch.detach();//ワンショットタイマー停止
lilac0112_1	7:c418a0d929b1	174	Interrupt=OFF;
lilac0112_1	0:4baeb4381394	175	}
lilac0112_1	0:4baeb4381394	176	}
lilac0112_1	0:4baeb4381394	177	void LeftCatch(void){//左腕のリミットスイッチ
lilac0112_1	7:c418a0d929b1	178
lilac0112_1	7:c418a0d929b1	179	Hand[L][Close] = OFF;
lilac0112_1	7:c418a0d929b1	180
lilac0112_1	0:4baeb4381394	181	if((!HandSW[R])&&(!HandSW[L])){//どちらも押されたら停止
lilac0112_1	0:4baeb4381394	182	StopCatching();//停止
lilac0112_1	0:4baeb4381394	183	Touch.detach();//ワンショットタイマー停止
lilac0112_1	7:c418a0d929b1	184	Interrupt=OFF;
lilac0112_1	0:4baeb4381394	185	}
lilac0112_1	0:4baeb4381394	186	}
lilac0112_1	0:4baeb4381394	187	void CenterCatch(void){//中央のリミットスイッチ
lilac0112_1	7:c418a0d929b1	188
lilac0112_1	2:eb1c123c9140	189	if((!OneEighty[R])&&(!OneEighty[L])){
lilac0112_1	7:c418a0d929b1	190	Interrupt=ON;
lilac0112_1	2:eb1c123c9140	191	Touch.attach(StopCatching, TIME_LIMIT);//TIME_LIMIT[s]で停止
lilac0112_1	2:eb1c123c9140	192	StartCatching();
lilac0112_1	2:eb1c123c9140	193	}
lilac0112_1	7:c418a0d929b1	194	if(GetBit(INdata[0], 1)==true){
lilac0112_1	7:c418a0d929b1	195	StartCatching();
lilac0112_1	7:c418a0d929b1	196	}
lilac0112_1	0:4baeb4381394	197	}
lilac0112_1	4:1102ab499430	198	uint8_t LinkBit(bool eight, bool seven, bool six, bool five, bool fore, bool three, bool two, bool one){
lilac0112_1	4:1102ab499430	199	return 0x80eight + 0x40seven + 0x20six + 0x10five + 0x08fore + 0x04three + 0x02two + 0x01one;
lilac0112_1	4:1102ab499430	200	}
lilac0112_1	4:1102ab499430	201	bool GetBit(uint8_t n, uint8_t bit){//n=8[bit]data, bit = 1-8(8bitのデータから指定bitの数値を読み取る)-
lilac0112_1	4:1102ab499430	202	return (n>>(bit-1))%2;
lilac0112_1	4:1102ab499430	203	}
lilac0112_1	1:006debaed874	204	void SetUp(void){//literally
lilac0112_1	1:006debaed874	205
lilac0112_1	5:c2a278842131	206	mt.mode(PullUp);
lilac0112_1	1:006debaed874	207
lilac0112_1	1:006debaed874	208	HandSpeed[R] = HandSpeed[L] = ARM_REV;
lilac0112_1	8:4f9c4b791bfe	209	TurnSpeed = TURN_REV;
lilac0112_1	1:006debaed874	210
lilac0112_1	7:c418a0d929b1	211	//HandSW[R].mode(PullUp);
lilac0112_1	7:c418a0d929b1	212	//HandSW[L].mode(PullUp);
lilac0112_1	7:c418a0d929b1	213	//HandSW[C].mode(PullUp);
lilac0112_1	7:c418a0d929b1	214	//HandSW[R].fall(RightCatch);
lilac0112_1	7:c418a0d929b1	215	//HandSW[L].fall(LeftCatch);
lilac0112_1	7:c418a0d929b1	216	//HandSW[C].fall(CenterCatch);
lilac0112_1	2:eb1c123c9140	217
lilac0112_1	2:eb1c123c9140	218	OneEighty[R].mode(PullUp);
lilac0112_1	2:eb1c123c9140	219	OneEighty[L].mode(PullUp);
lilac0112_1	2:eb1c123c9140	220
lilac0112_1	2:eb1c123c9140	221	TurnSW[N].mode(PullUp);
lilac0112_1	2:eb1c123c9140	222	TurnSW[E].mode(PullUp);
lilac0112_1	2:eb1c123c9140	223
lilac0112_1	7:c418a0d929b1	224	}
lilac0112_1	7:c418a0d929b1	225

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	12 Jul 2014
Imports:	7
Forks:	0
Commits:	12
Dependents:	0
Dependencies:	4
Followers:	9

main.cpp@8:4f9c4b791bfe, 2014-10-05 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning