โปรแกรมของหุ่นยนต์อัตโนมัติแบ่งเป็นสองไฟล์
ไฟล์แรกจะเป็นส่วน Timer interrupt และ multi state ส่วนไฟล์ที่สองจะเป็นส่วนโปรแกรมย่อย
File1
#define NTIMER 2
#define TIME_OF_SENSOR 1
#define TIME_OF_MOTOR 1
#define Orange_LEFT_SENSOR PORTC.F2
#define Orange_RIGHT_SENSOR PORTC.F3
#define BASKET_LEFT_SENSOR PORTC.F0
#define BASKET_RIGHT_SENSOR PORTC.F1
#define ARM_ROTATE_MOTOR1 PORTB.F6 //if ARM_ROTATE_MOTOR1=1; and ARM_ROTATE_MOTOR1=0; is Rotate left
#define ARM_ROTATE_MOTOR2 PORTB.F7
#define ARM_SLIDE_MOTOR1 PORTB.F4 //if ARM_SLIDE_MOTOR1=0 and ARM_SLIDE_MOTOR2=1; is Extude
#define ARM_SLIDE_MOTOR2 PORTB.F5
#define ARM_HOLD_MOTOR1 PORTB.F2 //if ARM_HOLD_MOTOR1=1; and ARM_HOLD_MOTOR1=0; is Down
#define ARM_HOLD_MOTOR2 PORTB.F3
#define ARM_ROTATE_LM_LEFT PORTC.F7
#define ARM_ROTATE_LM_RIGHT PORTB.F0
#define ARM_ROTATE_LM_CENTRE PORTB.F1
#define ARM_SLIDE_LM_MAX PORTC.F6
#define ARM_SLIDE_LM_MIN PORTC.F5
#define LeftMOTER1 PORTA.F0
#define LeftMOTER2 PORTA.F1
#define RightMOTER1 PORTA.F2
#define RightMOTER2 PORTA.F3
#define BasketLeft PORTC.F0
#define BasketRight PORTC.F1
#define Turn_Left 'm'
#define Turn_Right 'n'
#define Rotate_Left 'o'
#define Rotate_Right 'p'
#define Stop 'q'
#define Forward 'r'
#define T1Flag PIR1.F0
#define T1Enable PIE1.F0
#define StartStopT1 T1CON.F0
void Moving(char);
void ReadTrksr();
void Tracking();
void Put_Orange(int Basket);
char T=0;
int state=0;
int Finish=0;
int BasketEnb=1;
// 1 tick as 10 ms
int TmrEnb[NTIMER] = {1,1};
int TmrMod[NTIMER] = {1,1};
int TmrRel[NTIMER] = {TIME_OF_SENSOR, TIME_OF_MOTOR};
int TmrCnt[NTIMER] = {TIME_OF_SENSOR, TIME_OF_MOTOR};
int TmrRdy[NTIMER] = {0,0};
int TmrErr[NTIMER] = {0,0};
int main (void)
{
/*
Frequency of Clock is 8MHz
set prescale 1:8 then F = 8MHz/8 = 1MHz so T =1/f = 1/1MHz = 1uS
Require This program is check interrupt every 10mS
*/
INTCON = 0xC0; // Enable GIE, Peripharal interrupt
T1Enable = 1; // Enable Timer1 interrupt
StartStopT1 = 1; // Start Timer1
T1CON.F1 = 0; // Select TMR1 Clock Source as Internal instruction cycle clock (FOSC/4)
T1CON.F2 = 1;
T1CON.F4 = 0; // Set Timer1 Input Clock Prescale is 1:2
T1CON.F5 = 0;
T1CON.TMR1GE = 1;
ANSEL = 0; // Configure AN pins as digital I/O
ANSELH = 0;
// OSCCON.OSTS = 0; // Select Internal oscillator is used for system clock
// OSCCON.IRCF0 = 1; // Configure to use internal oscillator
// OSCCON.IRCF1 = 1;
// OSCCON.IRCF2 = 1;
// OSCCON.SCS = 0;
// C1ON_BIT = 0; // Disable Comparators
// C2ON_BIT = 0;
TRISA = 0xF0; // set RA0-RA3 to be output, RA3-RA7 to be input
TRISB = 0x03; // set RB0-RB7 to be input
TRISC = 0xFF; // set RC0 to be input, RC1-RA7 to be output
PORTA = 0; // initialize PORTA, PORTB, PORTC
PORTB = 0;
PORTC = 0;
while (1)
{
switch(state)
{
case 0:
T1Enable = 0;
if(TmrRdy[0])
{
TmrRdy[0] = 0;
TMR1H = 0x00;
TMR1L = 0xff;
ReadTrksr();
T1Enable = 1;
}
else if(TmrRdy[1])
{
TmrRdy[1] = 0;
TMR1H = 0x00;
TMR1L = 0xff;
if(T==0xf0)
{
Moving(Forward);
}
else
{
state=1;
}
T1Enable = 1;
}
else
{
T1Enable = 1;
}
break;
case 1:
T1Enable = 0;
if(TmrRdy[0])
{
TmrRdy[0] = 0;
TMR1H = 0x00;
TMR1L = 0xff;
ReadTrksr();
T1Enable = 1;
}
else if(TmrRdy[1])
{
TmrRdy[1] = 0;
TMR1H = 0x00;
TMR1L = 0xff;
if(T==0xF0)
{
state = 2;
}
else
{
Tracking();
}
T1Enable = 1;
}
else
{
T1Enable = 1;
}
break;
case 2:
T1Enable = 0;
if(TmrRdy[0])
{
TmrRdy[0] = 0;
TMR1H = 0x00;
TMR1L = 0xff;
ReadTrksr();
T1Enable = 1;
}
else if(TmrRdy[1])
{
TmrRdy[1] = 0;
TMR1H = 0x00;
TMR1L = 0xff;
if(T==0x20)
{
TmrEnb[0] = 0;
TmrEnb[1] = 0;
state=3;
}
else
{
Moving(Turn_Left);
}
T1Enable = 1;
}
else
{
T1Enable = 1;
}
break;
case 3:
T1Enable = 0;
Moving(Stop);
if(ARM_ROTATE_LM_RIGHT)
{
state = 4;
TmrEnb[0] = 1;
TmrEnb[1] = 1;
T1Enable = 1;
}
else
{
T1Enable = 1;
}
break;
case 4:
T1Enable = 0;
if(TmrRdy[0])
{
TmrRdy[0] = 0;
TMR1H = 0x00;
TMR1L = 0xff;
ReadTrksr();
T1Enable = 1;
}
else if(TmrRdy[1])
{
TmrRdy[1] = 0;
TMR1H = 0x00;
TMR1L = 0xff;
if((T==0xf0)||(T==0x70)||(T==0xE0))
{
state = 5;
}
else
{
Tracking();
}
T1Enable = 1;
}
else
{
T1Enable = 1;
}
break;
case 5: //First Cross
T1Enable = 0;
if(TmrRdy[0])
{
TmrRdy[0] = 0;
TMR1H = 0x00;
TMR1L = 0xff;
ReadTrksr();
T1Enable = 1;
}
else if(TmrRdy[1])
{
TmrRdy[1] = 0;
TMR1H = 0x00;
TMR1L = 0xff;
if(T!=0x00)
{
Moving(Turn_Right);
}
else
{
state=6;
}
T1Enable = 1;
}
else
{
T1Enable = 1;
}
break;
case 6:
T1Enable = 0;
if(BASKET_LEFT_SENSOR)
{
if(BasketEnb)
{
Moving(Stop);
Put_Orange(1);
BasketEnb=0;
}
}
if(TmrRdy[0])
{
TmrRdy[0] = 0;
TMR1H = 0x00;
TMR1L = 0xff;
ReadTrksr();
T1Enable = 1;
}
else if(TmrRdy[1])
{
TmrRdy[1] = 0;
TMR1H = 0x00;
TMR1L = 0xff;
if(T==0xF0)//((T==0xf0)||(T==0x70)||(T==0xE0))
{
state = 7;
BasketEnb=1;
}
else
{
Tracking();
}
T1Enable = 1;
}
else
{
T1Enable = 1;
}
break;
case 7: //Second Cross
T1Enable = 0;
if(TmrRdy[0])
{
TmrRdy[0] = 0;
TMR1H = 0x00;
TMR1L = 0xff;
ReadTrksr();
T1Enable = 1;
}
else if(TmrRdy[1])
{
TmrRdy[1] = 0;
TMR1H = 0x00;
TMR1L = 0xff;
if(T!=0x00)
{
Moving(Turn_Left);
}
else
{
state=8;
}
T1Enable = 1;
}
else
{
T1Enable = 1;
}
break;
case 8:
T1Enable = 0;
if(TmrRdy[0])
{
TmrRdy[0] = 0;
TMR1H = 0x00;
TMR1L = 0xff;
ReadTrksr();
T1Enable = 1;
}
else if(TmrRdy[1])
{
TmrRdy[1] = 0;
TMR1H = 0x00;
TMR1L = 0xff;
if((T==0xA0)||(T==0xE0))//((T==0xf0)||(T==0x70)||(T==0xE0))
{
state = 9;
}
else
{
Tracking();
}
T1Enable = 1;
}
else
{
T1Enable = 1;
}
break;
case 9: //Thirth Cross
T1Enable = 0;
if(TmrRdy[0])
{
TmrRdy[0] = 0;
TMR1H = 0x00;
TMR1L = 0xff;
ReadTrksr();
T1Enable = 1;
}
else if(TmrRdy[1])
{
TmrRdy[1] = 0;
TMR1H = 0x00;
TMR1L = 0xff;
if(T!=0x00)
{
Moving(Turn_Left);
}
else
{
state=10;
}
T1Enable = 1;
}
else
{
T1Enable = 1;
}
break;
case 10:
T1Enable = 0;
if(TmrRdy[0])
{
TmrRdy[0] = 0;
TMR1H = 0x00;
TMR1L = 0xff;
ReadTrksr();
T1Enable = 1;
}
else if(TmrRdy[1])
{
TmrRdy[1] = 0;
TMR1H = 0x00;
TMR1L = 0xff;
if((T==0xF0)||(T==0xE0))//((T==0xf0)||(T==0x70)||(T==0xE0))
{
state = 11;
}
else
{
Tracking();
}
T1Enable = 1;
}
else
{
T1Enable = 1;
}
break;
case 11: //Thirth Cross
T1Enable = 0;
if(TmrRdy[0])
{
TmrRdy[0] = 0;
TMR1H = 0x00;
TMR1L = 0xff;
ReadTrksr();
T1Enable = 1;
}
else if(TmrRdy[1])
{
TmrRdy[1] = 0;
TMR1H = 0x00;
TMR1L = 0xff;
if(T!=0x00)
{
Moving(Turn_Left);
}
else
{
state=12;
}
T1Enable = 1;
}
else
{
T1Enable = 1;
}
break;
case 12:
T1Enable = 0;
if(TmrRdy[0])
{
TmrRdy[0] = 0;
TMR1H = 0x00;
TMR1L = 0xff;
ReadTrksr();
T1Enable = 1;
}
else if(TmrRdy[1])
{
TmrRdy[1] = 0;
TMR1H = 0x00;
TMR1L = 0xff;
if((T==0xF0)||(T==0xE0))//((T==0xf0)||(T==0x70)||(T==0xE0))
{
state = 13;
}
else
{
Tracking();
}
T1Enable = 1;
}
else
{
T1Enable = 1;
}
break;
case 13: //Fourth Cross
T1Enable = 0;
if(TmrRdy[0])
{
TmrRdy[0] = 0;
TMR1H = 0x00;
TMR1L = 0xff;
ReadTrksr();
T1Enable = 1;
}
else if(TmrRdy[1])
{
TmrRdy[1] = 0;
TMR1H = 0x00;
TMR1L = 0xff;
if(T!=0x00)
{
Moving(Turn_Left);
}
else
{
state=14;
}
T1Enable = 1;
}
else
{
T1Enable = 1;
}
break;
case 14:
T1Enable = 0;
if(BASKET_RIGHT_SENSOR)
{
if(BasketEnb)
{
Moving(Stop);
Put_Orange(2);
BasketEnb=0;
}
}
if(TmrRdy[0])
{
TmrRdy[0] = 0;
TMR1H = 0x00;
TMR1L = 0xff;
ReadTrksr();
T1Enable = 1;
}
else if(TmrRdy[1])
{
TmrRdy[1] = 0;
TMR1H = 0x00;
TMR1L = 0xff;
if((T==0x30)||(T==0x70))//((T==0xf0)||(T==0x70)||(T==0xE0))
{
state = 15;
BasketEnb=1;
}
else
{
Tracking();
}
T1Enable = 1;
}
else
{
T1Enable = 1;
}
break;
case 15: //Fifth Cross
T1Enable = 0;
if(TmrRdy[0])
{
TmrRdy[0] = 0;
TMR1H = 0x00;
TMR1L = 0xff;
ReadTrksr();
T1Enable = 1;
}
else if(TmrRdy[1])
{
TmrRdy[1] = 0;
TMR1H = 0x00;
TMR1L = 0xff;
if(T!=0x00)
{
Moving(Turn_Right);
}
else
{
state=16;
}
T1Enable = 1;
}
else
{
T1Enable = 1;
}
break;
case 16:
T1Enable = 0;
if(TmrRdy[0])
{
TmrRdy[0] = 0;
TMR1H = 0x00;
TMR1L = 0xff;
ReadTrksr();
T1Enable = 1;
}
else if(TmrRdy[1])
{
TmrRdy[1] = 0;
TMR1H = 0x00;
TMR1L = 0xff;
if((T==0x30)||(T==0x70))//((T==0xf0)||(T==0x70)||(T==0xE0))
{
state = 17;
BasketEnb=1;
}
else
{
Tracking();
}
T1Enable = 1;
}
else
{
T1Enable = 1;
}
break;
case 17: //Fifth Cross
T1Enable = 0;
if(TmrRdy[0])
{
TmrRdy[0] = 0;
TMR1H = 0x00;
TMR1L = 0xff;
ReadTrksr();
T1Enable = 1;
}
else if(TmrRdy[1])
{
TmrRdy[1] = 0;
TMR1H = 0x00;
TMR1L = 0xff;
if(T!=0x00)
{
Moving(Turn_Right);
}
else
{
state=18;
}
T1Enable = 1;
}
else
{
T1Enable = 1;
}
break;
case 18:
T1Enable = 0;
if(TmrRdy[0])
{
TmrRdy[0] = 0;
TMR1H = 0x00;
TMR1L = 0xff;
ReadTrksr();
T1Enable = 1;
}
else if(TmrRdy[1])
{
TmrRdy[1] = 0;
TMR1H = 0x00;
TMR1L = 0xff;
if((T==0x30)||(T==0x70))//((T==0xf0)||(T==0x70)||(T==0xE0))
{
state = 19;
BasketEnb=1;
}
else
{
Tracking();
}
T1Enable = 1;
}
else
{
T1Enable = 1;
}
break;
case 19: //Fifth Cross
T1Enable = 0;
if(TmrRdy[0])
{
TmrRdy[0] = 0;
TMR1H = 0x00;
TMR1L = 0xff;
ReadTrksr();
T1Enable = 1;
}
else if(TmrRdy[1])
{
TmrRdy[1] = 0;
TMR1H = 0x00;
TMR1L = 0xff;
if(T!=0x00)
{
Moving(Turn_Right);
}
else
{
state=20;
}
T1Enable = 1;
}
else
{
T1Enable = 1;
}
break;
case 20:
T1Enable = 0;
if(TmrRdy[0])
{
TmrRdy[0] = 0;
TMR1H = 0x00;
TMR1L = 0xff;
ReadTrksr();
T1Enable = 1;
}
else if(TmrRdy[1])
{
TmrRdy[1] = 0;
TMR1H = 0x00;
TMR1L = 0xff;
if((T==0xf0)||(T==0x70)||(T==0xE0))
{
state = 21;
BasketEnb=1;
}
else
{
Tracking();
}
T1Enable = 1;
}
else
{
T1Enable = 1;
}
break;
case 21: //Fifth Cross
T1Enable = 0;
if(TmrRdy[0])
{
TmrRdy[0] = 0;
TMR1H = 0x00;
TMR1L = 0xff;
ReadTrksr();
T1Enable = 1;
}
else if(TmrRdy[1])
{
TmrRdy[1] = 0;
TMR1H = 0x00;
TMR1L = 0xff;
if(T!=0x00)
{
Moving(Turn_Right);
}
else
{
state=22;
}
T1Enable = 1;
}
else
{
T1Enable = 1;
}
break;
case 22:
T1Enable = 0;
if(BASKET_RIGHT_SENSOR)
{
if(BasketEnb)
{
Moving(Stop);
Put_Orange(3);
BasketEnb=0;
}
}
if(TmrRdy[0])
{
TmrRdy[0] = 0;
TMR1H = 0x00;
TMR1L = 0xff;
ReadTrksr();
T1Enable = 1;
}
else if(TmrRdy[1])
{
TmrRdy[1] = 0;
TMR1H = 0x00;
TMR1L = 0xff;
if((T==0xf0)||(T==0x70)||(T==0xE0))
{
TmrEnb[0]=0;
TmrEnb[1]=0;
state = 23;
}
else
{
Tracking();
}
T1Enable = 1;
}
else
{
T1Enable = 1;
}
break;
case 23:
if(!Finish)
{
T1Enable = 0;
Moving(Forward);
delay_ms(2000);
Moving(Stop);
Finish=1;
}
break;
}
}
return 0;
}
void interrupt()
{
if (T1Flag) // Detect over flow of Timer1
{ int k;
for(k=0; k {
if(TmrEnb[k] == 1)
{
TmrCnt[k]--;
if(TmrCnt[k] == 0)
{
if(TmrRdy[k] == 1)
{
TmrErr[k] = 0;
}
else
{
TmrRdy[k] = 1;
TmrCnt[k] = TmrRel[k];
if(TmrMod[k] == 0)
{
TmrEnb[k]=0;
}
}
}
}
}
}
T1Flag=0;
}
File2
#define Orange_LEFT_SENSOR PORTC.F2
#define Orange_RIGHT_SENSOR PORTC.F3
#define BASKET_LEFT_SENSOR PORTC.F0
#define BASKET_RIGHT_SENSOR PORTC.F1
#define ARM_ROTATE_MOTOR1 PORTB.F6 //if ARM_ROTATE_MOTOR1=1; and ARM_ROTATE_MOTOR1=0; is Rotate left
#define ARM_ROTATE_MOTOR2 PORTB.F7
#define ARM_SLIDE_MOTOR1 PORTB.F4 //if ARM_SLIDE_MOTOR1=0 and ARM_SLIDE_MOTOR2=1; is Extude
#define ARM_SLIDE_MOTOR2 PORTB.F5
#define ARM_HOLD_MOTOR1 PORTB.F2 //if ARM_HOLD_MOTOR1=1; and ARM_HOLD_MOTOR1=0; is Down
#define ARM_HOLD_MOTOR2 PORTB.F3
#define ARM_ROTATE_LM_LEFT PORTC.F7
#define ARM_ROTATE_LM_RIGHT PORTB.F0
#define ARM_ROTATE_LM_CENTRE PORTB.F1
#define ARM_SLIDE_LM_MAX PORTC.F6
#define ARM_SLIDE_LM_MIN PORTC.F5
#define LEFT 1
#define RIGHT 2
#define LeftMOTER1 PORTA.F0
#define LeftMOTER2 PORTA.F1
#define RightMOTER1 PORTA.F2
#define RightMOTER2 PORTA.F3
#define BasketLeft PORTC.F0
#define BasketRight PORTC.F1
#define Turn_Left 'm'
#define Turn_Right 'n'
#define Rotate_Left 'o'
#define Rotate_Right 'p'
#define Stop 'q'
#define Forward 'r'
#define T1Flag PIR1.F0
#define T1Enable PIE1.F0
#define StartStopT1 T1CON.F0
#define Slide_Extude 's'
#define Slide_Cringe 't'
#define Slide_Stop 'u'
#define Swing_Left 'v'
#define Swing_Right 'w'
#define Swing_Stop 'x'
#define Hold_Up 'y'
#define Hold_Down 'z'
#define Hold_Stop 'a'
void Arm_Motor(char);
void Moving(char);
void ReadTrksr();
void Tracking();
void Put_Orange(int Basket);
extern char T=0;
void ReadTrksr() // Read Tracking Sensors Function
{
T=PORTA&0xF0;
}
void Moving(char Cmd) // This Function is receive the command to control the Moving system
{ switch(Cmd)
{ case 'm' : LeftMOTER1=0;LeftMOTER2=0;RightMOTER1=1;RightMOTER2=0; break; //Turn Left
case 'n' : LeftMOTER1=1;LeftMOTER2=0;RightMOTER1=0;RightMOTER2=0; break; //Turn Right
case 'o' : LeftMOTER1=0;LeftMOTER2=1;RightMOTER1=1;RightMOTER2=0; break; //Rotate Left
case 'p' : LeftMOTER1=1;LeftMOTER2=0;RightMOTER1=0;RightMOTER2=1; break; //Rotate Right
case 'q' : LeftMOTER1=0;LeftMOTER2=0;RightMOTER1=0;RightMOTER2=0; break; //Stop moving
case 'r' : LeftMOTER1=1;LeftMOTER2=0;RightMOTER1=1;RightMOTER2=0; break; //Move Forward
}
}
void Arm_Motor(char Cmd) // This Function is receive the command to control the Moving system
{ switch(Cmd)
{ case 's' : ARM_SLIDE_MOTOR1=0;ARM_SLIDE_MOTOR2=1; break; //Extude
case 't' : ARM_SLIDE_MOTOR1=1;ARM_SLIDE_MOTOR2=0; break; //Cringe
case 'u' : ARM_SLIDE_MOTOR1=0;ARM_SLIDE_MOTOR2=0; break; //Stop
case 'v' : ARM_ROTATE_MOTOR1=1;ARM_ROTATE_MOTOR2=0; break; //Rotate Left
case 'w' : ARM_ROTATE_MOTOR1=0;ARM_ROTATE_MOTOR2=1; break; //Rotate Right
case 'x' : ARM_ROTATE_MOTOR1=0;ARM_ROTATE_MOTOR2=0; break; //Stop
case 'y' : ARM_HOLD_MOTOR1=0;ARM_HOLD_MOTOR2=1; break; //Up
case 'z' : ARM_HOLD_MOTOR1=1;ARM_HOLD_MOTOR2=0; break; //Down
case 'a' : ARM_HOLD_MOTOR1=0;ARM_HOLD_MOTOR2=0; break; //Stop
}
}
void Tracking()
{
switch(T)
{ case 0x00 : Moving(Forward); break; //0000
case 0x10 : Moving(Rotate_Right); break; //0001
case 0x20 : Moving(Turn_Right); break; //0010
case 0x30 : Moving(Turn_Right); break; //0011
case 0x40 : Moving(Turn_Left); break; //0100
case 0x50 : Moving(Turn_Right); break; //0101
case 0x60 : Moving(Forward); break; //0110
case 0x70 : Moving(Turn_Right); break; //0111
case 0x80 : Moving(Rotate_Left); break; //1000
case 0x90 : Moving(Forward); break; //1001
case 0xa0 : Moving(Turn_Left); break; //1010
case 0xb0 : Moving(Turn_Right); break; //1011
case 0xc0 : Moving(Turn_Left); break; //1100
case 0xd0 : Moving(Turn_Left); break; //1101
case 0xe0 : Moving(Turn_Left); break; //1110
}
}
void Put_Orange(int Basket)
{ delay_ms(2000);
if(Basket==1)
{
do
{
Arm_Motor(Slide_Cringe);
}while(ARM_SLIDE_LM_MIN!=1);
Arm_Motor(Slide_Stop);
Arm_Motor(Slide_Extude);
delay_ms(4500);
Arm_Motor(Slide_Stop);
Arm_Motor(Swing_Left);
delay_ms(800);
Arm_Motor(Swing_Stop);
Arm_Motor(Hold_Down);
delay_ms(1500);
Arm_Motor(Hold_Up);
delay_ms(1000);
Arm_Motor(Hold_Stop);
do
{
Arm_Motor(Swing_Left);
}while(ARM_ROTATE_LM_LEFT!=1);
Arm_Motor(Swing_Stop);
Arm_Motor(Hold_Up);
delay_ms(500);
Arm_Motor(Hold_Stop);
do
{
Arm_Motor(Swing_Right);
}while(ARM_ROTATE_LM_CENTRE!=1);
Arm_Motor(Swing_Stop);
do
{
Arm_Motor(Slide_Cringe);
}while(ARM_SLIDE_LM_MIN!=1);
Arm_Motor(Slide_Stop);
}
else if(Basket==2)
{
do
{
Arm_Motor(Slide_Cringe);
}while(ARM_SLIDE_LM_MIN!=1);
Arm_Motor(Slide_Stop);
Arm_Motor(Slide_Extude);
delay_ms(4500);
Arm_Motor(Slide_Stop);
Arm_Motor(Swing_Right);
delay_ms(800);
Arm_Motor(Swing_Stop);
Arm_Motor(Hold_Down);
delay_ms(1500);
Arm_Motor(Hold_Up);
delay_ms(1000);
Arm_Motor(Hold_Stop);
do
{
Arm_Motor(Swing_Right);
}while(ARM_ROTATE_LM_RIGHT!=1);
Arm_Motor(Swing_Stop);
Arm_Motor(Hold_Up);
delay_ms(500);
Arm_Motor(Hold_Stop);
do
{
Arm_Motor(Swing_Left);
}while(ARM_ROTATE_LM_CENTRE!=1);
Arm_Motor(Swing_Stop);
do
{
Arm_Motor(Slide_Cringe);
}while(ARM_SLIDE_LM_MIN!=1);
Arm_Motor(Slide_Stop);
}
else
{
do
{
Arm_Motor(Slide_Cringe);
}while(ARM_SLIDE_LM_MIN!=1);
Arm_Motor(Slide_Stop);
Arm_Motor(Slide_Extude);
delay_ms(6000);
Arm_Motor(Slide_Stop);
Arm_Motor(Swing_Right);
delay_ms(800);
Arm_Motor(Swing_Stop);
Arm_Motor(Hold_Down);
delay_ms(1500);
Arm_Motor(Hold_Up);
delay_ms(1000);
Arm_Motor(Hold_Stop);
do
{
Arm_Motor(Swing_Right);
}while(ARM_ROTATE_LM_RIGHT!=1);
Arm_Motor(Swing_Stop);
Arm_Motor(Hold_Up);
delay_ms(500);
Arm_Motor(Hold_Stop);
do
{
Arm_Motor(Swing_Left);
}while(ARM_ROTATE_LM_CENTRE!=1);
Arm_Motor(Swing_Stop);//
Arm_Motor(Swing_Left);
delay_ms(800);
Arm_Motor(Swing_Stop);
Arm_Motor(Hold_Down);
delay_ms(1500);
Arm_Motor(Hold_Up);
delay_ms(1000);
Arm_Motor(Hold_Stop);
do
{
Arm_Motor(Swing_Right);
}while(ARM_ROTATE_LM_RIGHT!=1);
Arm_Motor(Swing_Stop);
Arm_Motor(Hold_Up);
delay_ms(500);
Arm_Motor(Hold_Stop);
do
{
Arm_Motor(Swing_Left);
}while(ARM_ROTATE_LM_CENTRE!=1);
Arm_Motor(Swing_Stop);//
do
{
Arm_Motor(Slide_Cringe);
}while(ARM_SLIDE_LM_MIN!=1);
Arm_Motor(Slide_Stop);
}
}
Online: 1
Visits: 21,438
Today: 2
PageView/Month: 50
ด้วยความปราถนาดีจาก "สยามทูเว็บดอทคอม" และเพื่อป้องกันการเปิดเว็บไซต์เพื่อหลอกลวงขายของ โปรดตรวจสอบร้านค้าให้แน่ใจก่อนตัดสินใจซื้อของทุกครั้งนะคะ อ่านเพิ่มเติม ...
