Bチーム
足の上げ下げを行う 一つの足につき2つのセンサを使用、 RTOS使用、 NHKの足上げプログラム予定
main.cpp
- Committer:
- ryuna
- Date:
- 2014-10-12
- Revision:
- 0:0773a5499020
File content as of revision 0:0773a5499020:
/*
*覚え書き
*
***mbedのピン配置に対するこのプログラムのピンの設定
*p5 ~ p12 Motor ...BusOut :Motor moved
*p13 ~ p20 Photo ...BusIn :Photo Sensor
*p21 ~ p24 Pwm ...PwmOut :Motor Pwm Signal
*p21 ~ p24 Sp ...DigitalOut :Check sensor state,do ditto led
*p25 ~ p28 Tempra...BusOut :Motor Tempra Moved
*p29,p30 Sw[2] ...DigitalIn :Switch Multi Use
*LED1 ~ LED4 Led ...DigitalOut :Led Brinked
*
**********************
***関数の内容
*void WaitStop() :use wait
*void MotorStop() :erase motor pulses
*void MotorStart() :motor start timing timer
*void TempraStop() :stop motor of crawler
* //void Photo() :main processing ,wheel(lift or lower)motor move start
*void Reset() :wheels lower
*void GainSetting():sensor check mode,blinking Led
*void AndCount() :and count
*void SetUp() :first in function
*int main() :main function
*
*スレッド
*void Photo01_Thread(void const *argument)
*void Photo23_Thread(void const *argument)
*void Photo45_Thread(void const *argument)
*void Photo67_Thread(void const *argument) : main processing ,wheel(lift or lower)motor move start thread
*void ParallelThread(void const *argument) : Jump reset or GainSetting function
*
***********************
***処理内容
*photo系
*前のセンサで検知したら足をあげる、その後後ろのセンサが検知したら足を下げる
*前センサ→前センサ→後ろセンサ→後ろセンサ この場合足の上げ下げは一回しか行われない
*
*
*
**追記
*Photo {右前々、右前後、右後前、右後々、 左前々、左前後、左後前、左後々}
*
*
*/
#include "mbed.h"
#include "rtos.h"
#define MOTOR_NUM 8 //motor & sensor num
#define START_TIME_B 0.237 //motor start wait time of Back //a lot
#define START_TIME_F 0.04
#define MOVE_TIME 0.10 //lift a wheel for 0.090 seconds
#define MOVE_DOWN_TIME 0.115 //lower a wheel for 0.10 seconds
#define TEMPRA_TIME 3.0 //move a crawler for 4.0 seconds
#define WAIT_TIME 0.010 //make wait time
#define PWM_LEVEL 1.0 //motors pwm setting level
BusIn Photo(p5, p6, p7, p8, p9, p10, p11, p12 );
DigitalIn Sw[2] = {p29, p30};
DigitalOut Led[4] = {LED1, LED2, LED3, LED4 };
DigitalOut Sp[4] = {p21, p22, p23, p24 };
BusOut Motor(p13, p14, p15, p16, p17, p18, p19, p20 );//{left front, right front, left back ，right back }
BusOut Tempra(p25, p26, p27, p28 );
//PwmOut Pwm[4] = {p21, p22, p23, p24 };
Timeout StartTimer[4];
Timeout StopTimer[8];
Timeout StopTempra;
Timeout StopWait[4];
volatile bool MotorState[4] = {0} ,Flag[4] = {0} ,FlagAdd[4] = {0} ,And[4] = {0} ,Wait[4] = {0};
volatile uint8_t Way = 0, DownCount = 0;
volatile bool TempraFlag = 1;
/*
Motor = {
LeftFrontUp, LeftFrontDown,
RightFrontUp, RightFrontDown,
LeftBackUp, LeftBackDown,
RightBackUp, RightBackDown
};
*/
/***********************WaitStop*/
void WaitStop0()
{
Wait[0] = 0;
}
void WaitStop1()
{
Wait[1] = 0;
}
void WaitStop2()
{
Wait[2] = 0;
}
void WaitStop3()
{
Wait[3] = 0;
}
/*
********************************MotorStop
*/
void MotorStop0()
{
Motor = Motor & 0x7F;
}
void MotorStop1()
{
Motor = Motor & 0xBF;
StopWait[0].attach(&WaitStop0,WAIT_TIME);
}
void MotorStop2()
{
Motor = Motor & 0xF7;
}
void MotorStop3()
{
Motor = Motor & 0xFB;
StopWait[1].attach(&WaitStop1,WAIT_TIME);
}
void MotorStop4()
{
Motor = Motor & 0xDF;
}
void MotorStop5()
{
Motor = Motor & 0xEF;
StopWait[2].attach(&WaitStop2,WAIT_TIME);
}
void MotorStop6()
{
Motor = Motor & 0xFD;
}
void MotorStop7()
{
Motor = Motor & 0xFE;
StopWait[3].attach(&WaitStop3,WAIT_TIME);
}
/*
***********************MotorStart
*/
void MotorStart0()
{
StopTimer[0].attach(&MotorStop0,MOVE_TIME);
Motor = Motor & 0xBF;
Motor = Motor | 0x80;//10000000
}
void MotorStart1()
{
StopTimer[2].attach(&MotorStop2,MOVE_TIME);
Motor = Motor & 0xFB;
Motor = Motor | 0x08;//00001000
}
void MotorStart2()
{
StopTimer[4].attach(&MotorStop4,MOVE_TIME);
Motor = Motor & 0xEF;
Motor = Motor | 0x20;//00100000
}
void MotorStart3()
{
StopTimer[6].attach(&MotorStop6,MOVE_TIME);
Motor = Motor & 0xFE;
Motor = Motor | 0x02;//00000010
}
void TempraStop()
{
Tempra = 0x00;
}
void Photo01_Thread(void const *argument)
{
uint8_t CkPhoto = 0;
bool CkFlag[2] = {1,1};
while(1) {
CkPhoto = (~Photo) & 0x03;
if(CkPhoto==0) { // 右前足
CkFlag[0] = 1;
CkFlag[1] = 1;
} else if(CkPhoto<3) {
CkPhoto -= 1;
if(CkFlag[CkPhoto]) {
if(CkPhoto == 0) {
if(Way ==0){
Way = 1;
}
if((Way == 1)&&!Wait[0]) {
if(!MotorState[0]) {
StartTimer[0].attach(&MotorStart0,START_TIME_F);
MotorState[0] = 1;
Led[0] = 1;
Sp [0] = 1;
} else {
Flag[0] = 1;
}
}
} else {
if(Way == 1) {
if(Flag[0]&&!FlagAdd[0]) {
FlagAdd[0] = 1;
} else if(MotorState[0]) {
StopTimer[1].attach(&MotorStop1,MOVE_DOWN_TIME);
Motor = Motor & 0x7F;// 01111111
Motor = Motor | 0x40;//0x40 = 01000000
MotorState[0] = 0;
Flag[0] = 0;
FlagAdd[0] = 0;
Led[0] = 0;
Sp [0] = 0;
And[0] = 1;
Wait[0] = 1;
}
}
}
CkFlag[CkPhoto] = 0;
}
}
}
}
void Photo23_Thread(void const *argument)
{
uint8_t CkPhoto = 0;
bool CkFlag[2] = {1,1};
while(1) {
CkPhoto = ((~Photo) & 0x0C) >> 2;
if(CkPhoto==0) { // 右前足
CkFlag[0] = 1;
CkFlag[1] = 1;
} else if(CkPhoto<3) {
CkPhoto -= 1;
if(CkFlag[CkPhoto]) {
if(CkPhoto == 0) {
if((Way == 1)&&!Wait[1]) {
if(!MotorState[1]) {
StartTimer[1].attach(&MotorStart1,START_TIME_B);
MotorState[1] = 1;
Led[1] = 1;
Sp [1] = 1;
} else {
Flag[1] = 1;
}
}
} else {
if(Way == 1) {
if(Flag[1]&&!FlagAdd[1]) {
FlagAdd[1] = 1;
} else if(MotorState[1]) {
StopTimer[3].attach(&MotorStop3,MOVE_DOWN_TIME);
Motor = Motor & 0xF7;
Motor = Motor | 0x04;//00000100
MotorState[1] = 0;
Flag[1] = 0;
FlagAdd[1] = 0;
Led[1] = 0;
Sp [1] = 0;
And[1] = 1;
Wait[1] = 1;
}
}
}
CkFlag[CkPhoto] = 0;
}
}
}
}
void Photo45_Thread(void const *argument)
{
uint8_t CkPhoto = 0;
bool CkFlag[2] = {1,1};
while(1) {
CkPhoto = ((~Photo) & 0x30) >> 4;
if(CkPhoto==0) { // 右前足
CkFlag[0] = 1;
CkFlag[1] = 1;
} else if(CkPhoto<3) {
CkPhoto -= 1;
if(CkFlag[CkPhoto]) {
if(CkPhoto == 0) {
if(Way == 0) {
Way = 1;
}
if((Way == 1)&&!Wait[2]) {
if(!MotorState[2]) {
StartTimer[2].attach(&MotorStart2,START_TIME_F);
MotorState[2] = 1;
Led[2] = 1;
Sp [2] = 1;
} else {
Flag[2] = 1;
}
}
} else {
if(Way == 1) {
if(Flag[2]&&!FlagAdd[2]) {
FlagAdd[2] = 1;
} else if(MotorState[2]) {
StopTimer[5].attach(&MotorStop5,MOVE_DOWN_TIME);
Motor = Motor & 0xDF;
Motor = Motor | 0x10;//00010000
MotorState[2] = 0;
Flag[2] = 0;
FlagAdd[2] = 0;
Led[2] = 0;
Sp [2] = 0;
And[2] = 1;
Wait[2] = 1;
}
}
}
CkFlag[CkPhoto] = 0;
}
}
}
}
void Photo67_Thread(void const *argument)
{
uint8_t CkPhoto = 0;
bool CkFlag[2] = {1,1};
while(1) {
CkPhoto = ((~Photo) & 0xC0) >> 6;
if(CkPhoto==0) { // 右前足
CkFlag[0] = 1;
CkFlag[1] = 1;
} else if(CkPhoto<3) {
CkPhoto -= 1;
if(CkFlag[CkPhoto]) {
if(CkPhoto == 0) {
if((Way == 1)&&!Wait[3]) {
if(!MotorState[3]) {
StartTimer[3].attach(&MotorStart3,START_TIME_B);
MotorState[3] = 1;
Led[3] = 1;
Sp [3] = 1;
} else {
Flag[3] = 1;
}
}
} else {
if(Way == 1) {
if(Flag[3]&&!FlagAdd[3]) {
FlagAdd[3] = 1;
} else if(MotorState[3]) {
StopTimer[7].attach(&MotorStop7,MOVE_DOWN_TIME);
Motor = Motor & 0xFD;//11111101
Motor = Motor | 0x01;//00000001
MotorState[3] = 0;
Flag[3] = 0;
FlagAdd[3] = 0;
Led[3] = 0;
Sp [3] = 0;
And[3] = 1;
Wait[3] = 1;
}
}
}
CkFlag[CkPhoto] = 0;
}
}
}
}
void Reset()
{
int i;
if(MotorState[0]) {
StopTimer[1].attach(&MotorStop1,MOVE_DOWN_TIME);
Motor = Motor | 0x40;//01000000
}
if(MotorState[1]) {
StopTimer[3].attach(&MotorStop3,MOVE_DOWN_TIME);
Motor = Motor | 0x04;//00000100
}
if(MotorState[2]) {
StopTimer[5].attach(&MotorStop5,MOVE_DOWN_TIME);
Motor = Motor | 0x10;//00010000
}
if(MotorState[3]) {
StopTimer[7].attach(&MotorStop7,MOVE_DOWN_TIME);
Motor = Motor | 0x01;//00000001
}
for(i = 0; i < 4; i++) {
Led[i] = 0;
Flag[i] = 0;
FlagAdd[i] = 0;
MotorState[i] = 0;
And[i] = 0;
}
Way = 0;
DownCount = 0;
TempraFlag = 1;
}
void GainSetting ()
{
bool CkFlag[8] = {1,1,1,1,1,1,1,1},PhotoCk[8] = {0};
Reset();
Tempra = 0;
TempraFlag = 0;
Led[0] = Led[1] = Led[2] = Led[3] = 1;
Sp[0] = Sp[1] = Sp[2] = Sp[3] = 1;
while(1) {
if(!Sw[0]) {
while(1) {//右側のセンサの調整
PhotoCk[0] = ((~Photo)&0x01);
PhotoCk[1] = ((~Photo)&0x02) >> 1;
PhotoCk[2] = ((~Photo)&0x04) >> 2;
PhotoCk[3] = ((~Photo)&0x08) >> 3;
if(PhotoCk[0]&&CkFlag[0]) {
Led[0] = 0;
Sp [0] = 0;
CkFlag[0] = 0;
} else if(!PhotoCk[0]) {
CkFlag[0] = 1;
Led[0] = 1;
Sp [0] = 1;
}
if(PhotoCk[1]&&CkFlag[1]) {
Led[1] = 0;
Sp [1] = 0;
CkFlag[1] = 0;
} else if(!PhotoCk[1]) {
CkFlag[1] = 1;
Led[1] = 1;
Sp [1] = 1;
}
if(PhotoCk[2]&&CkFlag[2]) {
Led[2] = 0;
Sp [2] = 0;
CkFlag[2] = 0;
} else if(!PhotoCk[2]) {
CkFlag[2] = 1;
Led[2] = 1;
Sp [2] = 1;
}
if(PhotoCk[3]&&CkFlag[3]) {
Led[3] = 0;
Sp [3] = 0;
CkFlag[3] = 0;
} else if(!PhotoCk[3]) {
CkFlag[3] = 1;
Led[3] = 1;
Sp [3] = 1;
}
if(!Sw[1]) break;
}
}
//左側のセンサの調整
PhotoCk[4] = ((~Photo)&0x10) >> 4;
PhotoCk[5] = ((~Photo)&0x20) >> 5;
PhotoCk[6] = ((~Photo)&0x40) >> 6;
PhotoCk[7] = ((~Photo)&0x80) >> 7;
if(PhotoCk[4]&&CkFlag[4]) {
Led[0] = 0;
Sp [0] = 0;
CkFlag[4] = 0;
} else if(!PhotoCk[4]) {
CkFlag[4] = 1;
Led[0] = 1;
Sp [0] = 1;
}
if(PhotoCk[5]&&CkFlag[5]) {
Led[1] = 0;
Sp [1] = 0;
CkFlag[5] = 0;
} else if(!PhotoCk[5]) {
CkFlag[5] = 1;
Led[1] = 1;
Sp [1] = 1;
}
if(PhotoCk[6]&&CkFlag[6]) {
Led[2] = 0;
Sp [2] = 0;
CkFlag[6] = 0;
} else if(!PhotoCk[6]) {
CkFlag[6] = 1;
Led[2] = 1;
Sp [2] = 1;
}
if(PhotoCk[7]&&CkFlag[7]) {
Led[3] = 0;
Sp [3] = 0;
CkFlag[7] = 0;
} else if(!PhotoCk[7]) {
CkFlag[7] = 1;
Led[3] = 1;
Sp [3] = 1;
}
}
}
void AndCount() //"Wheels" Up and Down counting
{
DownCount ++;
And[0] = 0 ;
And[1] = 0 ;
And[2] = 0 ;
And[3] = 0 ;
if(DownCount > 3) {
DownCount = 0;
Way = 0;
}
}
/****************************Reset or Crawler_stop or CheckMode_Start Thread
*/
void ParallelThread(void const *argument)
{
uint8_t Ct;
while (1) {
if(And[0]&&And[1]&&And[2]&&And[3]) {
AndCount();
}
if((MotorState[0]||MotorState[1]||MotorState[2]||MotorState[3])&&TempraFlag) {
Tempra = 0x0A;
StopTempra.attach(&TempraStop,TEMPRA_TIME);
}
if(!Sw[0]&&!Sw[1]) {
Ct = 0;
while(!Sw[0]&&!Sw[1]&&(Ct <= 100)) {
Ct++;
}
if(Ct >= 100) {
GainSetting();
}
} else if(!Sw[0]) {
Ct = 0;
while(!Sw[0]&&(Ct <= 100)) {
Ct ++;
}
if(Ct >= 100) {
Reset();
}
} else if(!Sw[1]) {
Ct = 0;
while(!Sw[1]&&(Ct <= 100)) {
Ct ++;
}
if(Ct >= 100) {
Tempra = 0;
TempraFlag = 0;
}
}
}
}
void SetUp()
{
/*
Pwm[0] = PWM_LEVEL ;
Pwm[1] = PWM_LEVEL ;
Pwm[2] = PWM_LEVEL ;
Pwm[3] = PWM_LEVEL ;
*/
Sp[0] = Sp[1] = Sp[2] = Sp[3] = 0;
Photo.mode(PullUp);
Sw[0].mode(PullUp);
Sw[1].mode(PullUp);
}
int main()
{
SetUp();
Thread thread(ParallelThread);
Thread thread0(Photo01_Thread);
Thread thread1(Photo23_Thread);
Thread thread2(Photo45_Thread);
Thread thread3(Photo67_Thread);
wait(0.5);
while(1) {
}
}