/********************************************************************************************************
Function Name: MotorInit
Description :
Inputs : None
Outputs : None
Notes :
Revision :
********************************************************************************************************/
void MotorInit(void)
{
MotorIOInit();
MotorTIMInit();
SetMotor1PWM(0);
SetMotor2PWM(0);
}
/********************************************************************************************************
Function Name: Test_Motor
Description : µç»ú²âÊÔ
Inputs : None
Outputs : None
Notes :
Revision :
********************************************************************************************************/
void Test_Motor(void)
{
while(1)
{
Test_Motor_PWM = v1;
SetMotor1PWM(Test_Motor_PWM);
SetMotor2PWM(Test_Motor_PWM);
}
}
void Test_Main(void)
{
/*
int read_tempe=0;
//ÏÔʾÊý¾Ý
UpdataZLG7290(4, 0, 0) ;Delay_ms(3000);
UpdataZLG7290(5, 0, 0) ;Delay_ms(3000);
UpdataZLG7290(6, 0, 0) ;Delay_ms(3000);
UpdataZLG7290(7, 0, 0) ;Delay_ms(3000);
Delay_ms(5000);
//Çå³ýÊý¾Ý
ZLG7290ClearAll() ;
Delay_ms(5000);
while(1)
{
while(1)
{
while(ReadKeys() == 16){} //°´¼üûÓа´ÏÂʱ һֱѻ·É¨Ãè Ö±µ½ ReadKeys() £¡= 16
read_tempe=ReadKeys();
while(ReadKeys() != 16);//°´¼üûËÉ¿ªÊ±£¬µÈ´ý¡£¡£¡£¡£
switch(read_tempe)
{
case 1:
{
UpdataZLG7290(4, 0, 0) ;Delay_ms(2000);
UpdataZLG7290(5, 0, 0) ;Delay_ms(2000);
UpdataZLG7290(6, 0, 0) ;Delay_ms(2000);
UpdataZLG7290(7, 1, 0) ;Delay_ms(2000);
Test_Sover_PWM_L=ReadKeyAndDisplay();
Delay_ms(2000);
break;
}
case 2:
{
UpdataZLG7290(4, 0, 0) ;Delay_ms(2000);
UpdataZLG7290(5, 0, 0) ;Delay_ms(2000);
UpdataZLG7290(6, 0, 0) ;Delay_ms(2000);
UpdataZLG7290(7, 2, 0) ;Delay_ms(2000);
Test_Sover_PWM_R=ReadKeyAndDisplay();
Delay_ms(2000);
break;
}
case 3:
{
UpdataZLG7290(4, 0, 0) ;Delay_ms(2000);
UpdataZLG7290(5, 0, 0) ;Delay_ms(2000);
UpdataZLG7290(6, 0, 0) ;Delay_ms(2000);
UpdataZLG7290(7, 3, 0) ;Delay_ms(2000);
Test_Motor_PWM_L=ReadKeyAndDisplay()-2000;
Delay_ms(2000);
break;
}
case 4:
{
UpdataZLG7290(4, 0, 0) ;Delay_ms(2000);
UpdataZLG7290(5, 0, 0) ;Delay_ms(2000);
UpdataZLG7290(6, 0, 0) ;Delay_ms(2000);
UpdataZLG7290(7, 4, 0) ;Delay_ms(2000);
Test_Motor_PWM_R=ReadKeyAndDisplay()-2000;
Delay_ms(2000);
break;
}
}
SetServo1PWM(Test_Sover_PWM_L);
SetServo2PWM(Test_Sover_PWM_R);
SetMotor1PWM(Test_Motor_PWM_L);
SetMotor2PWM(Test_Motor_PWM_R);
Test_count++;if(Test_count>=4000)Test_count=0;
}
}
*/
HeadLeft_Sensor = 0;
HeadRight_Sensor = 0;
BackLeft_Sensor = 0;
BackRight_Sensor = 0;
Head_Sensor = 1;
Back_Sensor = 1;
Left_Sensor = 1;
Right_Sensor = 1;
v_Rejust = 128;
v0 = 0;
v1 = 512;
v2 = 768;
v3 = 1280;
v_Max = 1536;
isDelayed = 1;
isFirstRun = 1;
while(1)
{
HeadLeft_Sensor = GetKeyState(1);
HeadRight_Sensor = GetKeyState(2);
BackLeft_Sensor = GetKeyState(3);
BackRight_Sensor = GetKeyState(4);
if(HeadLeft_Sensor == 0 && HeadRight_Sensor == 0 && BackLeft_Sensor == 0 && BackRight_Sensor == 0) {
if(isFirstRun == 1) {
UINT16 i = 0;
UINT16 j = 0;
for(i = 0; i < 2000; i++) // delay 3s
{
for(j = 0; j < 1040; j++)
{
SetMotor1PWM(v2);
SetMotor2PWM(v2);
}
if(GetKeyState(1) == 1 || GetKeyState(2) == 1)
break;
}
isFirstRun = 0;
}
else if(isFirstRun == 0) {
//int state_value = 0;
Test_Motor_PWM = v2;
SetMotor1PWM(v2);
SetMotor2PWM(v2);
/*state_value = Current_State(HeadLeft_Sensor, HeadRight_Sensor, BackLeft_Sensor, BackRight_Sensor);
if(state_value == 1)
Escape(1, 1500, -v2 - v_Rejust, -v2);
else if(state_value == 2)
Escape(2, 1500, v2, v2 + v_Rejust);
else if(state_value == 3)
Escape(2, 1500, v2, v2);
else if(state_value == 4)
Escape(1, 1500, -v2, -v2);
else if(state_value == 5)
Escape(2, 1500, v2, v2 + v_Rejust);
else if(state_value == 6)
Escape(2, 1500, v2 + v_Rejust, v2);
else if(state_value == 7)
Escape(2, 1500, v2, v2 + v_Rejust);
else if(state_value == 8)
Escape(2, 1500, v2 + v_Rejust, v2);
else if(state_value == 9)
Escape(2, 1500, v2, v2 + v_Rejust);
else if(state_value == 10)
Escape(2, 1500, v2, v2);*/
}
}
// double sensors
else if(HeadLeft_Sensor == 1 && HeadRight_Sensor == 1 && BackLeft_Sensor == 0 && BackRight_Sensor == 0) {
int state_value = 0;
UINT16 i = 0;
UINT16 j = 0;
isDelayed = 0;
Test_Motor_PWM_L = -v1;
Test_Motor_PWM_R = -v1;
Test_Motor_PWM = -v1;
for(i = 0; i < 2500; i++) // delay
{
if(i > 1000) { // forbidden head to right
Test_Motor_PWM_L = Test_Motor_PWM - v_Rejust;
Test_Motor_PWM_R = Test_Motor_PWM + v_Rejust;
}
for(j = 0; j < 1040; j++)
{
// waiting for rejusting
SetMotor1PWM(Test_Motor_PWM_L);
SetMotor2PWM(Test_Motor_PWM_R);
}
state_value = Current_State(HeadLeft_Sensor, HeadRight_Sensor, BackLeft_Sensor, BackRight_Sensor);
if(state_value == 15) // kill all
Escape(0, 1500, v_Max, -v_Max);
else if(state_value == 11 || state_value == 13) // back and right
Escape(1, 1500, -v2, -v2 - v_Rejust * 2);
else if(state_value == 12) // left
Escape(1, 1500, -v2 - v_Rejust * 2, -v2);
if(GetKeyState(3) == 1 || GetKeyState(4) == 1)
break;
}
}
else if(BackLeft_Sensor == 1 && BackRight_Sensor == 1 && HeadLeft_Sensor == 0 && HeadRight_Sensor == 0) {
int state_value = 0;
UINT16 i = 0;
UINT16 j = 0;
isDelayed = 0;
Test_Motor_PWM_L = v1;
Test_Motor_PWM_R = v1;
Test_Motor_PWM = v1;
for(i = 0; i < 2500; i++) // delay
{
if(i > 1000) { // forbidden head to left
Test_Motor_PWM_L = Test_Motor_PWM + v_Rejust;
Test_Motor_PWM_R = Test_Motor_PWM - v_Rejust;
}
for(j = 0; j < 1040; j++)
{
// waiting for rejusting
SetMotor1PWM(Test_Motor_PWM_L);
SetMotor2PWM(Test_Motor_PWM_R);
}
state_value = Current_State(HeadLeft_Sensor, HeadRight_Sensor, BackLeft_Sensor, BackRight_Sensor);
if(state_value == 25) // kill all
Escape(0, 1500, v_Max, -v_Max);
else if(state_value == 21 || state_value == 22) // left and head
Escape(2, 1500, v2 + v_Rejust * 2, v2);
else if(state_value == 23) // right
Escape(2, 1500, v2, v2 + v_Rejust * 2);
if(GetKeyState(1) == 1 || GetKeyState(2) == 1)
break;
}
}
else if(HeadLeft_Sensor == 1 && BackLeft_Sensor == 1 && HeadRight_Sensor == 0 && BackRight_Sensor == 0) {
int state_value = 0;
isDelayed = 0;
Test_Motor_PWM_L = v1;
Test_Motor_PWM_R = v0;
SetMotor1PWM(Test_Motor_PWM_L);
SetMotor2PWM(Test_Motor_PWM_R);
state_value = Current_State(HeadLeft_Sensor, HeadRight_Sensor, BackLeft_Sensor, BackRight_Sensor);
if(state_value == 35)
Escape(0, 1500, v_Max, -v_Max);
else if(state_value == 31)
Escape(2, 1500, v2 + v_Rejust, v2 - v_Rejust);
else if(state_value == 32 || state_value == 33)
Escape(2, 1500, v2 + v_Rejust * 2, v2);
}
else if(HeadRight_Sensor == 1 && BackRight_Sensor == 1 && HeadLeft_Sensor == 0 && BackLeft_Sensor == 0) {
int state_value = 0;
isDelayed = 0;
Test_Motor_PWM_L = v1;
Test_Motor_PWM_R = v0;
SetMotor1PWM(Test_Motor_PWM_L);
SetMotor2PWM(Test_Motor_PWM_R);
state_value = Current_State(HeadLeft_Sensor, HeadRight_Sensor, BackLeft_Sensor, BackRight_Sensor);
if(state_value == 45)
Escape(0, 1500, v_Max, -v_Max);
else if(state_value == 41)
Escape(2, 1500, v2 - v_Rejust, v2 + v_Rejust);
else if(state_value == 42 || state_value == 43)
Escape(2, 1500, v2, v2 + v_Rejust * 2);
}
// single sensor
else if(BackLeft_Sensor == 0 && HeadLeft_Sensor == 1 && BackRight_Sensor == 0 && HeadRight_Sensor == 0) {
int state_value = 0;
UINT16 i = 0;
UINT16 j = 0;
isDelayed = 0;
Test_Motor_PWM_L = -v1;
Test_Motor_PWM_R = v0;
for(i = 0; i < 2500; i++)
{
if(i > 1000)
Test_Motor_PWM_R = -v1;
for(j = 0; j < 1040; j++)
{
SetMotor1PWM(Test_Motor_PWM_L);
SetMotor2PWM(Test_Motor_PWM_R);
}
state_value = Current_State(HeadLeft_Sensor, HeadRight_Sensor, BackLeft_Sensor, BackRight_Sensor);
if(state_value == 56) // kill all
Escape(0, 1500, v_Max, -v_Max);
else if(state_value == 51 || state_value == 53 || state_value == 54)
Escape(3, 1500, -v3, -v3);
else if(state_value == 52)
Escape(3, 1500, -v2 - v_Rejust * 4, -v2);
if(GetKeyState(2) == 1 || GetKeyState(3) == 1 || GetKeyState(4) == 1)
break;
}
}
else if(BackRight_Sensor == 0 && HeadRight_Sensor == 1 && BackLeft_Sensor == 0 && HeadLeft_Sensor == 0) {
int state_value = 0;
UINT16 i = 0;
UINT16 j = 0;
isDelayed = 0;
Test_Motor_PWM_L = v0;
Test_Motor_PWM_R = -v1;
for(i = 0; i < 2500; i++)
{
if(i > 1000)
Test_Motor_PWM_L = -v1;
for(j = 0; j < 1040; j++)
{
SetMotor1PWM(Test_Motor_PWM_L);
SetMotor2PWM(Test_Motor_PWM_R);
}
state_value = Current_State(HeadLeft_Sensor, HeadRight_Sensor, BackLeft_Sensor, BackRight_Sensor);
if(state_value == 66) // kill all
Escape(0, 1500, v_Max, -v_Max);
else if(state_value == 61 || state_value == 63 || state_value == 64)
Escape(4, 1500, -v3, -v3);
else if(state_value == 62)
Escape(4, 1500, -v2, -v2 - v_Rejust * 4);
if(GetKeyState(1) == 1 || GetKeyState(3) == 1 || GetKeyState(4) == 1)
break;
}
}
else if(HeadRight_Sensor == 0 && HeadLeft_Sensor == 0 && BackRight_Sensor == 0 && BackLeft_Sensor == 1) {
int state_value = 0;
UINT16 i = 0;
UINT16 j = 0;
isDelayed = 0;
Test_Motor_PWM_L = v1;
Test_Motor_PWM_R = v0;
for(i = 0; i < 2500; i++)
{
if(i > 1000)
Test_Motor_PWM_R = v1;
for(j = 0; j < 1040; j++)
{
SetMotor1PWM(Test_Motor_PWM_L);
SetMotor2PWM(Test_Motor_PWM_R);
}
state_value = Current_State(HeadLeft_Sensor, HeadRight_Sensor, BackLeft_Sensor, BackRight_Sensor);
if(state_value == 76) // kill all
Escape(0, 1500, v_Max, -v_Max);
else if(state_value == 72 || state_value == 73 || state_value == 74)
Escape(5, 1500, v3, v3);
else if(state_value == 71)
Escape(5, 1500, v2 + v_Rejust * 4, v2);
if(GetKeyState(1) == 1 || GetKeyState(2) == 1 || GetKeyState(4) == 1)
break;
}
}
else if(HeadLeft_Sensor == 0 && HeadRight_Sensor == 0 && BackLeft_Sensor == 0 && BackRight_Sensor == 1) {
int state_value = 0;
UINT16 i = 0;
UINT16 j = 0;
isDelayed = 0;
Test_Motor_PWM_L = v0;
Test_Motor_PWM_R = v1;
for(i = 0; i < 2500; i++)
{
if(i > 1000)
Test_Motor_PWM_L = v1;
for(j = 0; j < 1040; j++)
{
SetMotor1PWM(Test_Motor_PWM_L);
SetMotor2PWM(Test_Motor_PWM_R);
}
state_value = Current_State(HeadLeft_Sensor, HeadRight_Sensor, BackLeft_Sensor, BackRight_Sensor);
if(state_value == 86) // kill all
Escape(0, 1500, v_Max, -v_Max);
else if(state_value == 82 || state_value == 83 || state_value == 84)
Escape(6, 1500, v3, v3);
else if(state_value == 81)
Escape(6, 1500, v2, v2 + v_Rejust * 4);
if(GetKeyState(1) == 1 || GetKeyState(2) == 1 || GetKeyState(3) == 1)
break;
}
}
/*else if(HeadLeft_Sensor == 0 && HeadRight_Sensor == 0 && BackLeft_Sensor == 0 && BackRight_Sensor == 0) {
if(isDelayed == 1) { // run from static
if(isFirstRun == 1) {
UINT16 i = 0;
UINT16 j = 0;
for(i = 0; i < 2000; i++) // delay 3s
{
for(j = 0; j < 1040; j++)
{
SetMotor1PWM(v1);
SetMotor2PWM(v1);
}
//if(GetKeyState(1) != 0 || GetKeyState(2) != 0 || GetKeyState(3) != 0 || GetKeyState(4) != 0)
//break;
}
isFirstRun = 0;
}
else if(isFirstRun == 0) {
//int state_value = 0;
//Test_Motor_PWM = v2;
SetMotor1PWM(v2);
SetMotor2PWM(v2);
state_value = Current_State(HeadLeft_Sensor, HeadRight_Sensor, BackLeft_Sensor, BackRight_Sensor);
if(state_value == 1)
Escape(1, 1500, -v2 - v_Rejust, -v2);
else if(state_value == 2)
Escape(2, 1500, v2, v2 + v_Rejust);
else if(state_value == 3)
Escape(2, 1500, v2, v2);
else if(state_value == 4)
Escape(1, 1500, -v2, -v2);
else if(state_value == 5)
Escape(2, 1500, v2, v2 + v_Rejust);
else if(state_value == 6)
Escape(2, 1500, v2 + v_Rejust, v2);
else if(state_value == 7)
Escape(2, 1500, v2, v2 + v_Rejust);
else if(state_value == 8)
Escape(2, 1500, v2 + v_Rejust, v2);
else if(state_value == 9)
Escape(2, 1500, v2, v2 + v_Rejust);
else if(state_value == 10)
Escape(2, 1500, v2, v2);
}
}
else if(isDelayed == 0) { // not yet delayed
int state_value = 0;
UINT16 i = 0;
UINT16 j = 0;
Test_Motor_PWM = v1;
for(i = 0; i < 500; i++) // delay to rejust
{
for(j = 0; j < 1040; j++)
{
// waiting for down
SetMotor1PWM(Test_Motor_PWM);
SetMotor2PWM(Test_Motor_PWM);
}
state_value = Current_State(HeadLeft_Sensor, HeadRight_Sensor, BackLeft_Sensor, BackRight_Sensor);
if(state_value == 1)
Escape(1, 1500, -v2 - v_Rejust, -v2);
else if(state_value == 2)
Escape(2, 1500, v2, v2 + v_Rejust);
else if(state_value == 3)
Escape(2, 1500, v2, v2);
else if(state_value == 4)
Escape(1, 1500, -v2, -v2);
else if(state_value == 5)
Escape(2, 1500, v2, v2 + v_Rejust);
else if(state_value == 6)
Escape(2, 1500, v2 + v_Rejust, v2);
else if(state_value == 7)
Escape(2, 1500, v2, v2 + v_Rejust);
else if(state_value == 8)
Escape(2, 1500, v2 + v_Rejust, v2);
else if(state_value == 9)
Escape(2, 1500, v2, v2 + v_Rejust);
else if(state_value == 10)
Escape(2, 1500, v2, v2);
else
Escape(0, 1500, v_Max, -v_Max);
}
isDelayed = 1;
}
}*/
else { // impossible by nature
UINT16 i = 0;
UINT16 j = 0;
isDelayed = 0;
Test_Motor_PWM = v0;
SetMotor1PWM(Test_Motor_PWM);
SetMotor2PWM(Test_Motor_PWM);
if(Current_State(HeadLeft_Sensor, HeadRight_Sensor, BackLeft_Sensor, BackRight_Sensor) == -1) {
Test_Motor_PWM = v3;
for(i = 0; i < 1500; i++) // delay 3s
{
for(j = 0; j < 1040; j++)
{
SetMotor1PWM(Test_Motor_PWM);
SetMotor2PWM(-Test_Motor_PWM);
}
}
if(GetKeyState(1) == 1 || GetKeyState(2) == 1 )
break;
}
}
}
}
void Escape(UINT8 mode, UINT16 time, UINT16 v_left, UINT16 v_right)
{
UINT16 I = 0;
UINT16 J = 0;
if(mode == 0) {
for(I = 0; I < time; I++) {
for(J = 0; J < 1040; J++) {
SetMotor1PWM(v_left);
SetMotor2PWM(v_right);
}
}
}
else if(mode == 1) {
for(I = 0; I < time; I++) {
for(J = 0; J < 1040; J++) {
SetMotor1PWM(v_left);
SetMotor2PWM(v_right);
}
if(GetKeyState(3) == 1 || GetKeyState(4) == 1)
break;
}
}
else if(mode == 2) {
for(I = 0; I < time; I++) {
for(J = 0; J < 1040; J++) {
SetMotor1PWM(v_left);
SetMotor2PWM(v_right);
}
if(GetKeyState(1) == 1 || GetKeyState(2) == 1)
break;
}
}
// single process
else if(mode == 3) { // headleft
for(I = 0; I < time; I++) {
for(J = 0; J < 1040; J++) {
SetMotor1PWM(v_left);
SetMotor2PWM(v_right);
}
if(GetKeyState(2) == 1 || GetKeyState(3) == 1 || GetKeyState(4) == 1)
break;
}
}
else if(mode == 4) {
for(I = 0; I < time; I++) { // headright
for(J = 0; J < 1040; J++) {
SetMotor1PWM(v_left);
SetMotor2PWM(v_right);
}
if(GetKeyState(1) == 1 || GetKeyState(3) == 1 || GetKeyState(4) == 1)
break;
}
}
else if(mode == 5) { // backleft
for(I = 0; I < time; I++) {
for(J = 0; J < 1040; J++) {
SetMotor1PWM(v_left);
SetMotor2PWM(v_right);
}
if(GetKeyState(1) == 1 || GetKeyState(2) == 1 || GetKeyState(4) == 1)
break;
}
}
else if(mode == 6) { // backright
for(I = 0; I < time; I++) {
for(J = 0; J < 1040; J++) {
SetMotor1PWM(v_left);
SetMotor2PWM(v_right);
}
if(GetKeyState(1) == 1 || GetKeyState(2) == 1 || GetKeyState(3) == 1)
break;
}
}
}
