Bチーム
腕のプログラムです.
Dependencies: mbed TextLCD MultiSerial Pswitch
Diff: main.cpp
- Revision:
- 4:1102ab499430
- Parent:
- 3:a1489a94c436
- Child:
- 5:c2a278842131
--- a/main.cpp Wed Sep 03 07:44:20 2014 +0000
+++ b/main.cpp Tue Sep 30 12:28:33 2014 +0000
@@ -1,19 +1,26 @@
#include "mbed.h"
#include "MultiSerial.h"
-#define DATA_NUM 10 //DATA_NUM[byte]通信
+#define DATA_NUM 1 //DATA_NUM[byte]通信
#define MY_KEYCODE 0xAA//keycode
#define EN_KEYCODE 0xAA//keycode
#define KEY 0//number of first data
#define CHECK DATA_NUM-1//number of last data
#define TIME_LIMIT 1.0//腕が閉まっても動けるのはTIME_LIMIT[s]まで
-#define ARM_REV 1.0
-#define TURN_REV 0.5
+#define ARM_REV 1.0//腕のPWM
+#define TURN_REV 0.0//回転の初期PWM
+#define TURN_RATIO 1000000000//なるべく大きい数
+#define TURN_RATE 1//加速度の変更...1, 2, 4, 8, 16...
+#define TURN_REV_LIMIT 0.5//PWMの最大数値
+#define TURN_ACCELERATION 0.015625//なるべく小さい数
+#define TURN_SLOPE TURN_RATE*0.015625//なるべく小さい数1/2, 4, 8,...256
#define KEYCODE 0xAA
-volatile uint8_t INdata[DATA_NUM]={0}, EXdata[DATA_NUM]={0};
-volatile uint8_t count=0;
+uint8_t INdata[DATA_NUM]={0}, EXdata[DATA_NUM]={0};
+uint8_t count=0;
+uint8_t TURN, IO,SIG;
+uint8_t data[DATA_NUM];
enum Logic{OFF=0, ON};//殆どはモーターのON，OFFに使う
enum Arm{R=0, L, C};//右腕と左腕
@@ -21,10 +28,6 @@
enum Revolution{CW=0, CCW};//正回転，逆回転(上体回転)
enum Compass{N=0, E, W, S};//マシンの回転
-bool GetBit(uint8_t n, uint8_t bit){//n=8[bit]data, bit = 1-8(8bitのデータから指定bitの数値を読み取る)-
- return (n>>(bit-1))%2;
-}
-
BusOut LED(LED1, LED2, LED3, LED4);//確認用
/*
@@ -33,8 +36,7 @@
MultiSerial leg_mbed(p13,p14,read);
-uint8_t data[1];
-
+//BusIn mt(p5, p6, p7, p8);
//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
@@ -47,8 +49,10 @@
//ex)Turn[CW]=ON;
Serial pc(USBTX, USBRX);
Ticker flick;
+Ticker Accelerator;
Timeout Touch;//腕で掴むときの安全対策
+void Acceleration(void);
void StopCatching(void);//literally
void StopLeaving(void);//literally
void StartCatching(void);//literally
@@ -57,27 +61,32 @@
void LeftCatch(void);//左腕のリミットスイッチ
void CenterCatch(void);//中央のリミットスイッチ
void illumination(void);//literally
+uint8_t LinkBit(bool eight, bool seven, bool six, bool five, bool fore, bool three, bool two, bool one);
+bool GetBit(uint8_t n, uint8_t bit);
void SetUp(void);//literally
+
int main(void) {
+ //mt.mode(PullUp);
+
SetUp();
while(1) {
leg_mbed.read_data(data,KEYCODE);
- INdata[1]= data[0];
+ INdata[0] = data[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] = GetBit(INdata[1], 2)&&OneEighty[R];
- Hand[L][Open] = GetBit(INdata[1], 2)&&OneEighty[L];
+ Hand[R][Open] = GetBit(INdata[0], 2)&&OneEighty[R];
+ Hand[L][Open] = GetBit(INdata[0], 2)&&OneEighty[L];
}
}
@@ -96,22 +105,60 @@
// Touch.detach();//ワンショットタイマー停止
//}
- LED = ((0x01 * !HandSW[R]) | (0x02 * !HandSW[L]) | (0x04 * !HandSW[C]) | (0x08 * ((!OneEighty[R])&&(!OneEighty[L])) ));
+ //LED = ((0x01 * !HandSW[R]) | (0x02 * !HandSW[L]) | (0x04 * !HandSW[C]) | (0x08 * ((!OneEighty[R])&&(!OneEighty[L])) ));
+ LED = INdata[0];
//上体回転
- if(GetBit(INdata[1], 3)==OFF) Turn[CW] = OFF;//停止
- if(GetBit(INdata[1], 4)==OFF) Turn[CCW] = OFF;//停止
+ IO = GetBit(INdata[0], 3)+GetBit(INdata[0], 4);
+
+ //if(GetBit(INdata[0], 3)==OFF) Turn[CW] = OFF;//停止
+ //if(GetBit(INdata[0], 4)==OFF) Turn[CCW] = OFF;//停止
- if((GetBit(INdata[1], 3)+GetBit(INdata[1], 4))==1){//同時押し防止
-
- Turn[CW] = GetBit(INdata[1], 3)&&TurnSW[N];//押されていて，かつリミットスイッチが押されていなかったら右回転
- Turn[CCW] = GetBit(INdata[1], 4)&&TurnSW[E];//押されていて，かつリミットスイッチが押されていなかったら左回転
+ if((GetBit(INdata[0], 3)+GetBit(INdata[0], 4)==1)&&(TurnSpeed==0)){//同時押し防止
+
+ Turn[CW] = GetBit(INdata[0], 3)&&TurnSW[N];//押されていて，かつリミットスイッチが押されていなかったら右回転
+ Turn[CCW] = GetBit(INdata[0], 4)&&TurnSW[E];//押されていて，かつリミットスイッチが押されていなかったら左回転
+ TURN = LinkBit(0, 0, 0, 0, 0, 0, Turn[CW], Turn[CCW]);
}
+ if(TurnSpeed!=0){
+
+ Turn[CW] = GetBit(TURN, 2)&&TurnSW[N];
+ Turn[CCW] = GetBit(TURN, 1)&&TurnSW[E];
+
+ }
+ if((GetBit(INdata[0], 3)+GetBit(INdata[0], 4)!=1)&&(TurnSpeed==0)){
+ Turn[CW] = OFF;
+ Turn[CCW] = OFF;
+ }
+ if(!TurnSW[N]) Turn[CW]=OFF;//リミットスイッチによる停止
+ if(!TurnSW[E]) Turn[CCW]=OFF;
+
+
}
}
+void Acceleration(void){
+
+ float i;
+
+ SIG = LinkBit(0, 0, 0, 0, 0, 0, GetBit(INdata[0], 3), GetBit(INdata[0], 4));
+ IO = GetBit(INdata[0], 3)+GetBit(INdata[0], 4);
+
+ if((IO==1)&&(((unsigned long)(TurnSpeed*TURN_RATIO)==0)||(0<(unsigned long)(TurnSpeed*TURN_RATIO)))&&((unsigned long)(TurnSpeed*TURN_RATIO)<(unsigned long)(TURN_REV_LIMIT*TURN_RATIO))&&(TURN==SIG)){
+
+ TurnSpeed = TurnSpeed + TURN_SLOPE;
+
+ }
+ if(((IO==0)||(IO==2)||(TURN!=SIG))&&(0<(unsigned long)(TurnSpeed*TURN_RATIO))&&((unsigned long)(TurnSpeed*TURN_RATIO)<=(unsigned long)(TURN_REV_LIMIT*TURN_RATIO))){
+
+ TurnSpeed = TurnSpeed - TURN_SLOPE;
+
+ }
+ i = TurnSpeed;
+ pc.printf("%f, %d\n", i, IO);
+}
void StopCatching(void){//停止
Hand[R][Close]=Hand[L][Close]=OFF;
}
@@ -162,14 +209,21 @@
count++;
LED = count;
}
+uint8_t LinkBit(bool eight, bool seven, bool six, bool five, bool fore, bool three, bool two, bool one){
+ return 0x80*eight + 0x40*seven + 0x20*six + 0x10*five + 0x08*fore + 0x04*three + 0x02*two + 0x01*one;
+}
+bool GetBit(uint8_t n, uint8_t bit){//n=8[bit]data, bit = 1-8(8bitのデータから指定bitの数値を読み取る)-
+ return (n>>(bit-1))%2;
+}
void SetUp(void){//literally
- // mt.mode(PullUp);
+ //mt.mode(PullUp);
HandSpeed[R] = HandSpeed[L] = ARM_REV;
TurnSpeed = TURN_REV;
//flick.attach(illumination, .5);
+ Accelerator.attach(Acceleration, TURN_ACCELERATION);
HandSW[R].mode(PullUp);
HandSW[L].mode(PullUp);
@@ -186,4 +240,5 @@
TurnSW[N].mode(PullUp);
TurnSW[E].mode(PullUp);
+ wait(0.5);
}
\ No newline at end of file