Bチーム
腕のプログラムです.
Dependencies: mbed TextLCD MultiSerial Pswitch
Diff: main.cpp
- Revision:
- 7:c418a0d929b1
- Parent:
- 6:606507fc8852
- Child:
- 8:4f9c4b791bfe
--- a/main.cpp Fri Oct 03 12:30:58 2014 +0000
+++ b/main.cpp Sun Oct 05 06:50:36 2014 +0000
@@ -1,44 +1,43 @@
#include "mbed.h"
#include "MultiSerial.h"
-
+#include "Pswitch.h"
+
#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 KEYCODE 0xAA
+
+#define TIME_LIMIT 1.25//腕が閉まっても動けるのはTIME_LIMIT[s]まで
#define ARM_REV 1.0//腕のPWM
-#define TURN_REV 0.25//回転の初期PWM
-#define TURN_RATIO 100//1000000000//なるべく大きい数
+
+#define TURN_REV 0.2//回転の初期PWM
#define TURN_RATE 1//加速度の変更...1, 2, 4, 8, 16...
#define TURN_REV_LIMIT 0.5//PWMの最大数値
-#define TURN_ACCELERATION 0.1//0.015625//なるべく小さい数
-#define TURN_SLOPE TURN_RATE*0.1//0.015625//なるべく小さい数1/2, 4, 8,...256
+#define TURN_ACCELERATION (1e-4)/1.5//[s]
+#define TURN_RATIO 1e4//Holder/TURN_RATIO
-#define KEYCODE 0xAA
-
+
uint8_t INdata[DATA_NUM]={0}, EXdata[DATA_NUM]={0};
-uint8_t count=0;
+uint8_t Interrupt=0;
uint8_t TURN=0, IO=0,SIG=0;
uint8_t data[DATA_NUM];
+uint16_t Holder=0;
+#define COUNT_LIMIT 10e3
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);//Switch
*/
-
-
-BusIn mt(p7, p8, p9, p10);
-//DigitalIn Ninety[2]={p5, p6};//R, L
+
+
+BusIn mt(p5, p6, p7, p8);//mt(p7, p8);
DigitalIn OneEighty[2]={p18, p19};//R, L
-InterruptIn HandSW[3]={p15, p16, p17};//p14, p15, p16//R, L, C
+Pswitch HandSW[3]={p15, p16, p17};//p14, p15, p16//R, L, C
DigitalIn TurnSW[2]={p11, p12};//
PwmOut HandSpeed[2]={p23, p24};
PwmOut TurnSpeed(p22);//p22
@@ -47,12 +46,9 @@
DigitalOut Turn[2]={p29, p30};//p29, p30
//ex)Turn[CW]=ON;
Serial pc(USBTX, USBRX);
-Ticker flick;
-Ticker Save;
Ticker Accelerator;
Timeout Touch;//腕で掴むときの安全対策
-void SW_saving(void);
void Acceleration(void);
void StopCatching(void);//literally
void StopLeaving(void);//literally
@@ -61,17 +57,17 @@
void RightCatch(void);//右腕のリミットスイッチ
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) {
wait(0.5);
MultiSerial leg_mbed(p13,p14,read);
+ //Serial servo_mbed(p9, p10);
SetUp();
@@ -79,17 +75,27 @@
leg_mbed.read_data(data,KEYCODE);
- INdata[0] = data[0] | ~mt;
+ INdata[0] = data[0];// | ~mt;
+
+ //INdata[0] = ~mt;
+
+ //EXdata[0] = LinkBit(0, 0, 0, 0, 0, 0, !OneEighty[R], !OneEighty[L]);
+ //EXdata[0] = 3;//~mt;
+ LED = INdata[0];//~mt;//~mt;//EXdata[0]| servo_mbed.getc()*4;
+
+ //servo_mbed.putc(~mt);
+ //pc.printf("%5d, %f\n", Holder, TurnSpeed.read());
+
+ if((GetBit(INdata[0], 1)==OFF)&&(Interrupt==OFF)) StopCatching();
if(GetBit(INdata[0], 2)==OFF) StopLeaving();//停止
if((GetBit(INdata[0], 1)+GetBit(INdata[0], 2))==1){//同時押し防止
- if(GetBit(INdata[0], 1)==true){
- CenterCatch();//中央のリミットスイッチがうまく動作しなかった場合の保険
- }
+ if(GetBit(INdata[0], 1)==true) CenterCatch();//中央のリミットスイッチがうまく動作しなかった場合の保険
if(GetBit(INdata[0], 2)==true){//腕を広げる動作はマニュアル操作
StopCatching();//停止
Touch.detach();//ワンショットタイマー停止
+ Interrupt=OFF;
Hand[R][Open] = GetBit(INdata[0], 2)&&OneEighty[R];
Hand[L][Open] = GetBit(INdata[0], 2)&&OneEighty[L];
}
@@ -98,27 +104,35 @@
if(((!HandSW[R])&&(!HandSW[L]))&&((Hand[R][Close]&&Hand[L][Close])==ON)){//どちらも押されたら停止
StopCatching();//停止
Touch.detach();//ワンショットタイマー停止
+ Interrupt=OFF;
}
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(!HandSW[R]) Hand[R][Close]=OFF;
+ if(!HandSW[L]) Hand[L][Close]=OFF;
- //if((!Ninety[R])&&(!Ninety[L])){//どちらも押されたら停止
- // StopCatching();//停止
- // Touch.detach();//ワンショットタイマー停止
- //}
+ if(HandSW[R].count() > 0){
+ RightCatch();
+ }
+ if(HandSW[L].count() > 0){
+ LeftCatch();
+ }
+ if(HandSW[C].count() > 0){
+ CenterCatch();
+ }
//LED = ((0x01 * !HandSW[R]) | (0x02 * !HandSW[L]) | (0x04 * !HandSW[C]) | (0x08 * ((!OneEighty[R])&&(!OneEighty[L])) ));
LED = INdata[0];
//上体回転
- //IO = GetBit(INdata[0], 3)+GetBit(INdata[0], 4);
+ wait(TURN_ACCELERATION);
- //if(GetBit(INdata[0], 3)==OFF) Turn[CW] = OFF;//停止
- //if(GetBit(INdata[0], 4)==OFF) Turn[CCW] = OFF;//停止
+ Acceleration();
+
+ TurnSpeed.write( (float) ( (int)(100 * (TURN_REV_LIMIT*((float)(Holder)/TURN_RATIO)) ) /100.0 ) );
if((GetBit(INdata[0], 3)+GetBit(INdata[0], 4)==1)&&(TurnSpeed==0)){//同時押し防止
@@ -138,26 +152,6 @@
Turn[CCW] = OFF;
}
- //Acceleration();
-
- //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)&&(0<=TurnSpeed)&&(TurnSpeed<TURN_REV_LIMIT)&&(TURN==SIG)){
-
- TurnSpeed = TurnSpeed + TURN_SLOPE;
- wait(TURN_ACCELERATION);
- }
- if(((IO==0)||(IO==2)||(TURN!=SIG))&&(0<TurnSpeed)&&(TurnSpeed<=TURN_REV_LIMIT)){
-
- TurnSpeed = TurnSpeed - TURN_SLOPE;
- wait(TURN_ACCELERATION);
- }*/
- //i = TurnSpeed;
- //pc.printf("%f, %d\n", i, IO);
-
if(!TurnSW[N]) Turn[CW]=OFF;//リミットスイッチによる停止
if(!TurnSW[E]) Turn[CCW]=OFF;
@@ -165,46 +159,34 @@
}
}
-void SW_saving(void){
- IO = GetBit(INdata[0], 3)+GetBit(INdata[0], 4);
- SIG = LinkBit(0, 0, 0, 0, 0, 0, GetBit(INdata[0], 3), GetBit(INdata[0], 4));
-}
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;
- wait(TURN_ACCELERATION);
- }
- 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;
- wait(TURN_ACCELERATION);
- }
- i = TurnSpeed;
- pc.printf("%f, %d\n", i, IO);
+ if((IO==1)&&(Holder< 1e4)&&(TURN==SIG)) Holder++;
+ if(((IO!=1)||(TURN!=SIG))&&(0 <Holder)) Holder--;
+
}
void StopCatching(void){//停止
Hand[R][Close]=Hand[L][Close]=OFF;
+ if(Interrupt==ON) Interrupt=OFF;
}
void StopLeaving(void){//停止
Hand[R][Open]=Hand[L][Open]=OFF;
}
void StartCatching(void){//掴む（リミットスイッチが押されてなければ）
-
- if((!HandSW[R])||(!HandSW[L])){
+
+ if(GetBit(INdata[0], 1)==true){
+ Hand[R][Close]=HandSW[R];
+ Hand[L][Close]=HandSW[L];
+ }
+
+ 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;
@@ -216,32 +198,35 @@
Hand[L][Open]=OneEighty[L];
}
void RightCatch(void){//右腕のリミットスイッチ
+
+ Hand[R][Close] = OFF;
+
if((!HandSW[R])&&(!HandSW[L])){//どちらも押されたら停止
-
StopCatching();//停止
Touch.detach();//ワンショットタイマー停止
+ Interrupt=OFF;
}
}
void LeftCatch(void){//左腕のリミットスイッチ
+
+ Hand[L][Close] = OFF;
+
if((!HandSW[R])&&(!HandSW[L])){//どちらも押されたら停止
-
StopCatching();//停止
Touch.detach();//ワンショットタイマー停止
+ Interrupt=OFF;
}
}
void CenterCatch(void){//中央のリミットスイッチ
-
+
if((!OneEighty[R])&&(!OneEighty[L])){
+ Interrupt=ON;
Touch.attach(StopCatching, TIME_LIMIT);//TIME_LIMIT[s]で停止
StartCatching();
- LED=15;
}
-
-}
-
-void illumination(void){//literally
- count++;
- LED = count;
+ if(GetBit(INdata[0], 1)==true){
+ StartCatching();
+ }
}
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;
@@ -254,26 +239,22 @@
mt.mode(PullUp);
HandSpeed[R] = HandSpeed[L] = ARM_REV;
- TurnSpeed = TURN_REV;
+ //TurnSpeed = TURN_REV;
- //flick.attach(illumination, .5);
//Accelerator.attach(Acceleration, TURN_ACCELERATION);
- //Save.attach(SW_saving, 0.015625);
- HandSW[R].mode(PullUp);
- HandSW[L].mode(PullUp);
- HandSW[C].mode(PullUp);
- HandSW[R].fall(RightCatch);
- HandSW[L].fall(LeftCatch);
- HandSW[C].fall(CenterCatch);
- HandSW[C].rise(StopCatching);
+ //HandSW[R].mode(PullUp);
+ //HandSW[L].mode(PullUp);
+ //HandSW[C].mode(PullUp);
+ //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);
TurnSW[N].mode(PullUp);
TurnSW[E].mode(PullUp);
-}
\ No newline at end of file
+}
+
\ No newline at end of file