?? turnangle.c
字號:
#ifndef _TURNANGLE_
#define _TURNANGLE_
#include "HardwareInfo.c"
#include <GetCompassB.h>
#include <SetMotor.h>
#include "returnOut.c"
#include <SetCentiS.h>
void turnAngle(unsigned int angle, unsigned char yfy)
{
unsigned int counter = 0; //計數值。
unsigned int angle2 = 0; //存儲角度變量的值
unsigned int degree = 0; //指南針返回值。
unsigned char rotate = 0; //轉左?
unsigned char i = 0; //returnOut返回值
while ( counter<2 )
{
degree = GetCompassB(_COMPASS_jd_);
if ( degree>360 )
{
counter=0;
}
else
{
counter=counter+1;
}
}
if ( angle>=0&&angle<180 )
{
angle2=angle+180;
}
else
{
angle2=angle-180;
}
if ( ((degree>angle) && (degree< angle2) && (angle<angle2)) || (((degree>angle)&&(degree<=360)||(degree>=0)&&(degree<angle2)) && (angle>angle2)) )
{
rotate=1;
}
if ( rotate==1 )
{
SetMotor(_MOTOR_left_, 2, 45);
SetMotor(_MOTOR_right_, 0, 42);
}
else
{
SetMotor(_MOTOR_left_, 0, 35);
SetMotor(_MOTOR_right_, 2, 15);
}
while (1)
{
counter=0;
while ( counter<2 )
{
degree = GetCompassB(_COMPASS_jd_);
if ( degree>360 )
{
counter=0;
}
else
{
counter=counter+1;
}
}
if ( rotate>0 )
{
if ( (degree<=angle) && (angle-degree)>0 && (angle-degree)<5 )
{
break;
}
else
{
if ( ((360-degree+angle)>0) && ((360-degree+angle)<5) )
{
break;
}
}
}
else
{
if ( (degree>=angle)&&((degree-angle)>0 && (degree-angle)<5) )
{
break;
}
else
{
if ( ((360-angle+degree)>0)&&((360-angle+degree)<5) )
{
break;
}
}
}
i = returnOut(yfy);
if ( i==1 )
{
return ;
}
}
if ( rotate==1 )
{
SetMotor(_MOTOR_left_, 0, 50);
SetMotor(_MOTOR_right_, 2, 50);
}
else
{
SetMotor(_MOTOR_left_, 2, 50);
SetMotor(_MOTOR_right_, 0, 50);
}
SetCentiS(10);
SetMotor(_MOTOR_left_, 1, 0);
SetMotor(_MOTOR_right_, 1, 0);
}
#endif
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -