?? angle.c
字號:
#ifndef _ANGLE_
#define _ANGLE_
#include "HardwareInfo.c"
#include <GetCompassB.h>
#include <SetMotor.h>
#include <SetTenthS.h>
#include <SetLCD3Char.h>
void angle(unsigned int angle)
{
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, 30);
SetMotor(_MOTOR_right_, 0, 27);
}
else
{
SetMotor(_MOTOR_left_, 0, 25);
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;
}
}
}
}
if ( rotate==1 )
{
SetMotor(_MOTOR_left_, 0, 40);
SetMotor(_MOTOR_right_, 2, 40);
}
else
{
SetMotor(_MOTOR_left_, 2, 40);
SetMotor(_MOTOR_right_, 0, 40);
}
SetTenthS(2);
SetMotor(_MOTOR_left_, 1, 0);
SetMotor(_MOTOR_right_, 1, 0);
SetLCD3Char(1, degree);
}
#endif
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -