Bチーム
受け取りです．質問・アドバイスがあればどうぞ．
Dependencies: MultiSerial mbed
Diff: main.cpp
- Revision:
- 4:541fdc451ab3
- Parent:
- 3:d4eff9dace7d
diff -r d4eff9dace7d -r 541fdc451ab3 main.cpp
--- a/main.cpp Mon Jul 14 10:03:17 2014 +0000
+++ b/main.cpp Tue Sep 23 10:05:10 2014 +0000
@@ -1,45 +1,66 @@
#include "mbed.h"
-#include "main.h"//ピン位置，main関数までの初期動作の詳細，変数，関数等
-
-BusIn mt(p6, p7, p8, p9, p10, p11);
+#include "MultiSerial.h"
-extern bool GetBit(void);
-extern DigitalOut LED[4];
-//上体回転関連
-extern DigitalOut Turn[2];
-extern DigitalIn TurnSW;
-extern PwmOut TurnSpeed;
-//パワーウィンドウ(腕)関連
-extern DigitalOut Hand[2][2];
-extern InterruptIn HandSW[3];
-extern Timeout Touch;
-
-//通信関連
-extern Serial pc;
-extern Serial Xbee;//p13, p14
-extern volatile uint8_t INdata[DATA_NUM], EXdata[DATA_NUM];
-extern void TX(void);
-extern void RX(void);
-
-void SetUp(void);
+#define KEYCODE 0xAA
+#define DATA_NUM 1 //DATA_NUM[byte]通信
+#define TIME_LIMIT 1.0//腕が閉まっても動けるのはTIME_LIMIT[s]まで
+#define ARM_REV 1.0
+#define TURN_REV 0.25
+
+uint8_t INdata[DATA_NUM]={0}, EXdata[DATA_NUM]={0};
+volatile uint8_t count=0;
+
+enum Logic{OFF=0, ON};//殆どはモーターのON，OFFに使う
+enum Arm{R=0, L, C};//右腕と左腕
+enum Clip{Open=0, Close};//開ける動作と閉める動作
+enum Revolution{CW=0, CCW};//正回転，逆回転(上体回転)
+enum Compass{N=0, E, W, S};//マシンの回転
+
+BusOut LED(LED1, LED2, LED3, LED4);//確認用
+BusIn mt(p19, p20, p17, p18);
+//DigitalIn Ninety[2]={p5, p6};//R, L
+DigitalIn OneEighty[2]={p5, p6};//R, L
+InterruptIn HandSW[3]={p7, p8, p9};//p14, p15, p16//R, L, C
+DigitalIn TurnSW[2]={p11, p12};//
+PwmOut HandSpeed[2]={p23, p24};
+PwmOut TurnSpeed(p22);//p22
+DigitalOut Hand[2][2]={{p28, p27}, {p26, p25}};//{p17, p18}, {p19, p20}
+//ex)Hand[R][Close]=ON;
+DigitalOut Turn[2]={p29, p30};//p29, p30
+//ex)Turn[CW]=ON;
+Serial pc(USBTX, USBRX);
+MultiSerial Xbee(p13, p14, read);
+Ticker flick;
+Timeout Touch;//腕で掴むときの安全対策
+
+void StopCatching(void);//literally
+void StopLeaving(void);//literally
+void StartCatching(void);//literally
+void StartLeaving(void);//literally
+void RightCatch(void);//右腕のリミットスイッチ
+void LeftCatch(void);//左腕のリミットスイッチ
+void CenterCatch(void);//中央のリミットスイッチ
+void illumination(void);//literally
+void SetUp(void);//literally
+bool GetBit(uint8_t n, uint8_t bit);//literally
int main(void) {
SetUp();
while(1) {
- //腕のプログラム
- INdata[1]= ~mt;//調整用スイッチ
+ //INdata[0] = ~mt;
- if(GetBit(INdata[1], 2)==OFF) StopLeaving();//停止
+ if(GetBit(INdata[0], 2)==OFF) StopLeaving();//停止
- if((GetBit(INdata[1], 1)+GetBit(INdata[1], 2))==1){//同時押し防止
- if(GetBit(INdata[1], 1)==true) CenterCatch();//中央のリミットスイッチがうまく動作しなかった場合の保険
- if(GetBit(INdata[1], 2)==true){//腕を広げる動作はマニュアル操作
+ if((GetBit(INdata[0], 1)+GetBit(INdata[0], 2))==1){//同時押し防止
+ if(GetBit(INdata[0], 1)==true) CenterCatch();//中央のリミットスイッチがうまく動作しなかった場合の保険
+ if(GetBit(INdata[0], 2)==true){//腕を広げる動作はマニュアル操作
StopCatching();//停止
Touch.detach();//ワンショットタイマー停止
- Hand[R][Open] = Hand[L][Open] = GetBit(INdata[1], 2);
+ Hand[R][Open] = GetBit(INdata[0], 2)&&OneEighty[R];
+ Hand[L][Open] = GetBit(INdata[0], 2)&&OneEighty[L];
}
}
@@ -47,43 +68,109 @@
StopCatching();//停止
Touch.detach();//ワンショットタイマー停止
}
+
+ if(!OneEighty[R]) Hand[R][Open]=OFF;
+ if(!OneEighty[L]) Hand[L][Open]=OFF;
+ //if(!Ninety[R]) Hand[R][Close]=OFF;
+ //if(!Ninety[L]) Hand[L][Close]=OFF;
+
+ //if((!Ninety[R])&&(!Ninety[L])){//どちらも押されたら停止
+ // StopCatching();//停止
+ // Touch.detach();//ワンショットタイマー停止
+ //}
+
+ LED = ((0x01 * !HandSW[R]) | (0x02 * !HandSW[L]) | (0x04 * !HandSW[C]) | (0x08 * ((!OneEighty[R])&&(!OneEighty[L])) ));
+
//上体回転
- INdata[3] = ~mt;//調整用スイッチ
-
- Turn[CW] = Turn[CCW] = OFF;//停止
+ if(GetBit(INdata[0], 3)==OFF) Turn[CW] = OFF;//停止
+ if(GetBit(INdata[0], 4)==OFF) Turn[CCW] = OFF;//停止
- if((GetBit(INdata[3], 1)+GetBit(INdata[3], 2))==1){//同時押し防止
- if(GetBit(INdata[3], 1)&&(TurnSW)){
- Turn[CCW] = ON;//押されていて，かつリミットスイッチが押されていなかったら左回転
- }
+ if((GetBit(INdata[0], 3)+GetBit(INdata[0], 4))==1){//同時押し防止
- if(GetBit(INdata[3], 2)){
- Turn[CW] = ON;//押されたら右回転
- }
+ Turn[CW] = GetBit(INdata[0], 3)&&TurnSW[N];//押されていて，かつリミットスイッチが押されていなかったら右回転
+ Turn[CCW] = GetBit(INdata[0], 4)&&TurnSW[E];//押されていて，かつリミットスイッチが押されていなかったら左回転
+
}
}
}
-
-void SetUp(void){
+
+void StopCatching(void){//停止
+ Hand[R][Close]=Hand[L][Close]=OFF;
+}
+void StopLeaving(void){//停止
+ Hand[R][Open]=Hand[L][Open]=OFF;
+}
+void StartCatching(void){//掴む（リミットスイッチが押されてなければ）
+
+ if((!HandSW[R])&&(!HandSW[L])){
+ Hand[R][Close] = Hand[L][Close] = OFF;
+ }
+ else{
+
+ //Hand[R][Close]=Ninety[R];
+ //Hand[L][Close]=Ninety[L];
+
+ Hand[R][Close]=ON;
+ Hand[L][Close]=ON;
+
+ }
- uint8_t i=1;
+}
+void StartLeaving(void){//離す（リミットスイッチが押されてなければ）
+ Hand[R][Open]=OneEighty[R];
+ Hand[L][Open]=OneEighty[L];
+}
+void RightCatch(void){//右腕のリミットスイッチ
+ if((!HandSW[R])&&(!HandSW[L])){//どちらも押されたら停止
+ StopCatching();//停止
+ Touch.detach();//ワンショットタイマー停止
+ }
+}
+void LeftCatch(void){//左腕のリミットスイッチ
+ if((!HandSW[R])&&(!HandSW[L])){//どちらも押されたら停止
+ StopCatching();//停止
+ Touch.detach();//ワンショットタイマー停止
+ }
+}
+void CenterCatch(void){//中央のリミットスイッチ
+
+ if((!OneEighty[R])&&(!OneEighty[L])){
+ Touch.attach(StopCatching, TIME_LIMIT);//TIME_LIMIT[s]で停止
+ StartCatching();
+ }
+
+}
+void illumination(void){//literally
+ count++;
+ LED = count;
+}
+void SetUp(void){//literally
mt.mode(PullUp);
- TurnSpeed = TURN_REV;//上体回転のPWM
- TurnSW.mode(PullUp);
+ HandSpeed[R] = HandSpeed[L] = ARM_REV;
+ TurnSpeed = TURN_REV;
+
+ //flick.attach(illumination, .5);
HandSW[R].mode(PullUp);
HandSW[L].mode(PullUp);
HandSW[C].mode(PullUp);
- HandSW[R].rise(RightCatch);//立ち上がり
- HandSW[L].rise(LeftCatch);//立ち上がり
- HandSW[C].rise(CenterCatch);//立ち上がり
+ HandSW[R].fall(RightCatch);
+ HandSW[L].fall(LeftCatch);
+ HandSW[C].fall(CenterCatch);
+
+ OneEighty[R].mode(PullUp);
+ OneEighty[L].mode(PullUp);
+ //Ninety[R].mode(PullUp);
+ //Ninety[L].mode(PullUp);
- Xbee.attach(&RX, Serial::RxIrq);
- Xbee.attach(&TX, Serial::TxIrq);
+ TurnSW[N].mode(PullUp);
+ TurnSW[E].mode(PullUp);
- Xbee.putc(i);//送信開始
- i = Xbee.getc();//受信開始
-}
\ No newline at end of file
+ Xbee.read_data(INdata, KEYCODE);
+}
+bool GetBit(uint8_t n, uint8_t bit){//n=8[bit]data, bit = 1-8(8bitのデータから指定bitの数値を読み取る)
+ return (n>>(bit-1))%2;
+}
\ No newline at end of file