//************************************************************************************ //** //** File name: F:\AllFiles\Finish Prototypes\Tennis Ball Collector\Tennis Ball Collector Code\Ball_Collector.c //** Generated by: Flowcode v4.5.18.74 //** Date: Friday, January 25, 2013 01:40:03 //** Licence: Professional //** Registered to: azirr //** //** //** http://www.matrixmultimedia.com //************************************************************************************ #define MX_PIC //Defines for microcontroller #define P18F4550 #define MX_EE #define MX_EE_TYPE3 #define MX_EE_SIZE 256 #define MX_SPI #define MX_SPI_BCB #define MX_SPI_SDI 0 #define MX_SPI_SDO 7 #define MX_SPI_SCK 1 #define MX_UART #define MX_UART_C #define MX_UART_TX 6 #define MX_UART_RX 7 #define MX_I2C #define MX_MI2C #define MX_I2C_B #define MX_I2C_SDA 0 #define MX_I2C_SCL 1 #define MX_PWM #define MX_PWM_CNT 2 #define MX_PWM_TRIS1 trisc #define MX_PWM_1 2 #define MX_PWM_TRIS2 trisc #define MX_PWM_2 1 #define MX_PWM_TRIS2a trisb #define MX_PWM_2a 3 #define MX_USB_TYPE1 //Functions #define MX_CLK_SPEED 19660800 #ifdef _BOOSTC #include #endif #ifdef HI_TECH_C #include #endif //Configuration data #ifdef _BOOSTC #pragma DATA 0x300000, 0x4 #endif #ifdef _HI_TECH_C %C__CONFIG(%A, %V); #endif #ifdef _BOOSTC #pragma DATA 0x300001, 0xc #endif #ifdef _HI_TECH_C %C__CONFIG(%A, %V); #endif #ifdef _BOOSTC #pragma DATA 0x300002, 0x19 #endif #ifdef _HI_TECH_C %C__CONFIG(%A, %V); #endif #ifdef _BOOSTC #pragma DATA 0x300003, 0x0 #endif #ifdef _HI_TECH_C %C__CONFIG(%A, %V); #endif #ifdef _BOOSTC #pragma DATA 0x300004, 0xff #endif #ifdef _HI_TECH_C %C__CONFIG(%A, %V); #endif #ifdef _BOOSTC #pragma DATA 0x300005, 0x83 #endif #ifdef _HI_TECH_C %C__CONFIG(%A, %V); #endif #ifdef _BOOSTC #pragma DATA 0x300006, 0x85 #endif #ifdef _HI_TECH_C %C__CONFIG(%A, %V); #endif #ifdef _BOOSTC #pragma DATA 0x300007, 0xff #endif #ifdef _HI_TECH_C %C__CONFIG(%A, %V); #endif #ifdef _BOOSTC #pragma DATA 0x300008, 0xf #endif #ifdef _HI_TECH_C %C__CONFIG(%A, %V); #endif #ifdef _BOOSTC #pragma DATA 0x300009, 0xc0 #endif #ifdef _HI_TECH_C %C__CONFIG(%A, %V); #endif #ifdef _BOOSTC #pragma DATA 0x30000a, 0xf #endif #ifdef _HI_TECH_C %C__CONFIG(%A, %V); #endif #ifdef _BOOSTC #pragma DATA 0x30000b, 0xe0 #endif #ifdef _HI_TECH_C %C__CONFIG(%A, %V); #endif #ifdef _BOOSTC #pragma DATA 0x30000c, 0xf #endif #ifdef _HI_TECH_C %C__CONFIG(%A, %V); #endif #ifdef _BOOSTC #pragma DATA 0x30000d, 0x40 #endif #ifdef _HI_TECH_C %C__CONFIG(%A, %V); #endif //Internal functions #include "C:\Program Files\Matrix Multimedia\Flowcode V4\FCD\internals.h" //Macro function declarations void FCM_duration(); void FCM_Sonar_Sense(); void FCM_Movement(); void FCM_RIGHT_SONAR(); void FCM_CENTER_SONAR(); void FCM_LEFT_SONAR(); void FCM_FIND_MOVE(); void FCM_X_CENTERING(); void FCM_GET_BALL(); //Variable declarations char FCV_STATUS; short FCV_HOLD_CMP; char FCV_HEIGHT_SENSE; char FCV_STAT; short FCV_ECHO; char FCV_IN; short FCV_ECHO2; char FCV_IN2; short FCV_ECHO3; char FCV_IN3; char FCV_X_CENTERING; char FCV_SERVO1_POS; char FCV_BLOCKADE_SENSE; char FCV_RIGHT_FINDER; char FCV_TIMER; char FCLV_LOOP1; char FCLV_LOOP2; char FCLV_LOOP3; char FCLV_LOOP4; char FCLV_LOOP5; //Servo(0): //Defines: /**** Macro Substitutions **** 48 = Capture Compare High Byte Value 0 = Capture Compare Low Byte Value 0x01 = TIMER1 Configuration Register Value 0x0b = Capture Compare 1 Configuration Register Value 0x0a = Capture Compare 2 Configuration Register Value 3 = Number of Channels (1-8) 4,4,4,0,0,0,0,0 = Port Register Array ("0,0,1,1,3,2,0,1") 0,1,2,3,4,5,-1,-1 = Servo Pin Array ("0,1,0,1,0,0,2,2") 200,200,200,200,200,200,200,200 = Servo Trim Array ("128,128,128,128,128,128,128,128") 6291 = Number of clock cycles equivalent to 1280us ******************************/ //Common defines #define MX_SERVO_NO_CHAN 3 char MX_SERVO_PIN_ARRAY[8] = {0,1,2,3,4,5,-1,-1}; char MX_SERVO_TRIM[8] = {200,200,200,200,200,200,200,200}; #ifndef MX_MIAC_SYSTEM #define MX_SERVO_CCPR1H 48 #define MX_SERVO_CCPR1L 0 #define MX_SERVO_T1CON 0x01 #define MX_SERVO_CCP1CON 0x0b #define MX_SERVO_CCP2CON 0x0a //Global Variables char MX_SERVO_PORT_ARRAY[8] = {4,4,4,0,0,0,0,0}; char MX_SERVO_POSITION[8] = {0,0,0,0,0,0,0,0}; char MX_SERVO_REQUIRED[8] = {0,0,0,0,0,0,0,0}; unsigned int MX_SERVO_CLK_COUNT = 6291; char MX_SERVO_IDX = 0; unsigned long MX_SERVO_CALC = 0; #endif //Servo(0): //Macro function declarations void FCD_Servo0_TogglePort(char Port, char Pin, char State); void FCD_Servo0_ToggleTris(char Port, char Pin, char State); void FCD_Servo0_EnableServo(char Channel); void FCD_Servo0_DisableServo(char Channel); void FCD_Servo0_SetPosition(char Channel, char Position); void FCD_Servo0_SetTrim(char Channel, char Trim); char FCD_Servo0_MoveToPosition(char Channel, char Position); void FCD_Servo0_AutoMoveToPosition(char Channel, char Position, char StepSpeedms); //Servo(0): //Macro implementations void FCD_Servo0_TogglePort(char Port, char Pin, char State) { #ifndef MX_MIAC_SYSTEM if (Pin > 7) return; switch(Port) { #ifdef _BOOSTC case 0: #ifdef PORTA if(State) set_bit(porta, Pin); else clear_bit(porta, Pin); #endif #ifdef GPIO if(State) set_bit(gpio, Pin); else clear_bit(gpio, Pin); #endif break; case 1: #ifdef PORTB if(State) set_bit(portb, Pin); else clear_bit(portb, Pin); #endif break; case 2: #ifdef PORTC if(State) set_bit(portc, Pin); else clear_bit(portc, Pin); #endif break; case 3: #ifdef PORTD if(State) set_bit(portd, Pin); else clear_bit(portd, Pin); #endif break; case 4: #ifdef PORTE if(State) set_bit(porte, Pin); else clear_bit(porte, Pin); #endif break; case 5: #ifdef PORTF if(State) set_bit(portf, Pin); else clear_bit(portf, Pin); #endif break; #endif #ifdef HI_TECH_C case 0: #ifdef porta if(State) set_bit(porta, Pin); else clear_bit(porta, Pin); #endif #ifdef gpio if(State) set_bit(gpio, Pin); else clear_bit(gpio, Pin); #endif break; case 1: #ifdef portb if(State) set_bit(portb, Pin); else clear_bit(portb, Pin); #endif break; case 2: #ifdef portc if(State) set_bit(portc, Pin); else clear_bit(portc, Pin); #endif break; case 3: #ifdef portd if(State) set_bit(portd, Pin); else clear_bit(portd, Pin); #endif break; case 4: #ifdef porte if(State) set_bit(porte, Pin); else clear_bit(porte, Pin); #endif break; case 5: #ifdef portf if(State) set_bit(portf, Pin); else clear_bit(portf, Pin); #endif break; #endif } #endif } void FCD_Servo0_ToggleTris(char Port, char Pin, char State) { #ifndef MX_MIAC_SYSTEM if (Pin > 7) return; switch(Port) { #ifdef _BOOSTC case 0: #ifdef TRISA if(State) set_bit(trisa, Pin); else clear_bit(trisa, Pin); #endif #ifdef TRISIO if(State) set_bit(trisio, Pin); else clear_bit(trisio, Pin); #endif break; case 1: #ifdef TRISB if(State) set_bit(trisb, Pin); else clear_bit(trisb, Pin); #endif break; case 2: #ifdef TRISC if(State) set_bit(trisc, Pin); else clear_bit(trisc, Pin); #endif break; case 3: #ifdef TRISD if(State) set_bit(trisd, Pin); else clear_bit(trisd, Pin); #endif break; case 4: #ifdef TRISE if(State) set_bit(trise, Pin); else clear_bit(trise, Pin); #endif break; case 5: #ifdef TRISF if(State) set_bit(trisf, Pin); else clear_bit(trisf, Pin); #endif break; #endif #ifdef HI_TECH_C case 0: #ifdef trisa if(State) set_bit(trisa, Pin); else clear_bit(trisa, Pin); #endif #ifdef trisio if(State) set_bit(trisio, Pin); else clear_bit(trisio, Pin); #endif break; case 1: #ifdef trisb if(State) set_bit(trisb, Pin); else clear_bit(trisb, Pin); #endif break; case 2: #ifdef trisc if(State) set_bit(trisc, Pin); else clear_bit(trisc, Pin); #endif break; case 3: #ifdef trisd if(State) set_bit(trisd, Pin); else clear_bit(trisd, Pin); #endif break; case 4: #ifdef trise if(State) set_bit(trise, Pin); else clear_bit(trise, Pin); #endif break; case 5: #ifdef trisf if(State) set_bit(trisf, Pin); else clear_bit(trisf, Pin); #endif break; #endif } #endif } void FCD_Servo0_EnableServo(char Channel) { #ifdef MX_MIAC_SYSTEM #if (48 != MX_MIAC_MM_MIAC_BASE) unsigned int ModID = ((0 << 5) + 48) << 9; MIAC_CAN_TX[0] = Channel; MIAC_Module_WR(ModID, 641, 1,1,0); MIAC_Module_GetAck(ModID + 32, 641, 20); #endif #else if (Channel < MX_SERVO_NO_CHAN) FCD_Servo0_ToggleTris(MX_SERVO_PORT_ARRAY[Channel], MX_SERVO_PIN_ARRAY[Channel], 0); //Convert Servo pin to output to enable motor #endif } void FCD_Servo0_DisableServo(char Channel) { #ifdef MX_MIAC_SYSTEM #if (48 != MX_MIAC_MM_MIAC_BASE) unsigned int ModID = ((0 << 5) + 48) << 9; MIAC_CAN_TX[0] = Channel; MIAC_Module_WR(ModID, 642, 1,1,0); MIAC_Module_GetAck(ModID + 32, 642, 20); #endif #else if (Channel < MX_SERVO_NO_CHAN) FCD_Servo0_ToggleTris(MX_SERVO_PORT_ARRAY[Channel], MX_SERVO_PIN_ARRAY[Channel], 1); //Convert Servo pin to input to disable motor #endif } void FCD_Servo0_SetPosition(char Channel, char Position) { #ifdef MX_MIAC_SYSTEM #if (48 != MX_MIAC_MM_MIAC_BASE) unsigned int ModID = ((0 << 5) + 48) << 9; MIAC_CAN_TX[0] = Channel; MIAC_CAN_TX[1] = Position; MIAC_Module_WR(ModID, 643, 2,1,0); MIAC_Module_GetAck(ModID + 32, 643, 20); #endif #else if (Channel < 8) { MX_SERVO_POSITION[Channel] = Position; //Assign duty to motor controller logic MX_SERVO_REQUIRED[Channel] = Position; } #endif } void FCD_Servo0_SetTrim(char Channel, char Trim) { #ifdef MX_MIAC_SYSTEM #if (48 != MX_MIAC_MM_MIAC_BASE) unsigned int ModID = ((0 << 5) + 48) << 9; MIAC_CAN_TX[0] = Channel; MIAC_CAN_TX[1] = Trim; MIAC_Module_WR(ModID, 644, 2,1,0); MIAC_Module_GetAck(ModID + 32, 644, 20); #endif #else if (Channel < 8) MX_SERVO_TRIM[Channel] = Trim; //Assign duty to motor controller logic #endif } char FCD_Servo0_MoveToPosition(char Channel, char Position) { #ifdef MX_MIAC_SYSTEM #if (48 != MX_MIAC_MM_MIAC_BASE) unsigned int ModID = ((0 << 5) + 48) << 9; MIAC_CAN_TX[0] = Channel; MIAC_CAN_TX[1] = Position; MIAC_Module_WR(ModID, 645, 2,1,0); MIAC_Module_GetAck(ModID + 32, 645, 20); return(MIAC_CAN_RX[0]); #endif #else char arrived = 0; if (Channel < 8) { if (Position > MX_SERVO_POSITION[Channel]) MX_SERVO_POSITION[Channel] = MX_SERVO_POSITION[Channel] + 1; //Increment duty if (Position < MX_SERVO_POSITION[Channel]) MX_SERVO_POSITION[Channel] = MX_SERVO_POSITION[Channel] - 1; //Decrement duty if (Position == MX_SERVO_POSITION[Channel]) arrived = 1; MX_SERVO_REQUIRED[Channel] = MX_SERVO_POSITION[Channel]; } return arrived; #endif } void FCD_Servo0_AutoMoveToPosition(char Channel, char Position, char StepSpeedms) { #ifdef MX_MIAC_SYSTEM #if (48 != MX_MIAC_MM_MIAC_BASE) unsigned int ModID = ((0 << 5) + 48) << 9; MIAC_CAN_TX[0] = Channel; MIAC_CAN_TX[1] = Position; MIAC_Module_WR(ModID, 646, 2,1,0); MIAC_Module_GetAck(ModID + 32, 646, 20); #endif #else if (Channel < 8) MX_SERVO_REQUIRED[Channel] = Position; //Assign required duty #endif } //Macro implementations void FCM_duration() { //Output //Output: 0 -> PORTB trisb = 0x00; portb = 0; //Comment: //DOWN JOINT //Call Component Macro //Call Component Macro: Servo(0)::EnableServo(0) FCD_Servo0_EnableServo(0); //Calculation //Calculation: // servo1_Pos = 0 FCV_SERVO1_POS = 0; //Loop //Loop: While servo1_pos = 160 while (1) { //Calculation //Calculation: // servo1_Pos = servo1_Pos + 5 FCV_SERVO1_POS = FCV_SERVO1_POS + 5; //Call Component Macro //Call Component Macro: Servo(0)::SetPosition(0, servo1_Pos) FCD_Servo0_SetPosition(0, FCV_SERVO1_POS); //Delay //Delay: 100 ms delay_ms(100); if ((FCV_SERVO1_POS == 160) != 0) break; } //Call Component Macro //Call Component Macro: Servo(0)::DisableServo(0) FCD_Servo0_DisableServo(0); //Comment: //OPEN GRIP //Calculation //Calculation: // servo1_Pos = 0 FCV_SERVO1_POS = 0; //Call Component Macro //Call Component Macro: Servo(0)::EnableServo(2) FCD_Servo0_EnableServo(2); //Loop //Loop: While servo1_pos = 255 while (1) { //Calculation //Calculation: // servo1_Pos = servo1_Pos + 5 FCV_SERVO1_POS = FCV_SERVO1_POS + 5; //Call Component Macro //Call Component Macro: Servo(0)::SetPosition(2, servo1_Pos) FCD_Servo0_SetPosition(2, FCV_SERVO1_POS); //Delay //Delay: 100 ms delay_ms(100); if ((FCV_SERVO1_POS == 255) != 0) break; } //Call Component Macro //Call Component Macro: Servo(0)::DisableServo(2) FCD_Servo0_DisableServo(2); //Comment: //END GRIP //Comment: // DOWN BASE ARM //Calculation //Calculation: // servo1_Pos = 130 FCV_SERVO1_POS = 130; //Loop //Loop: While servo1_pos = 0 while (1) { //Calculation //Calculation: // servo1_Pos = servo1_Pos - 5 FCV_SERVO1_POS = FCV_SERVO1_POS - 5; //Call Component Macro //Call Component Macro: Servo(0)::SetPosition(1, servo1_Pos) FCD_Servo0_SetPosition(1, FCV_SERVO1_POS); //Delay //Delay: 100 ms delay_ms(100); if ((FCV_SERVO1_POS == 0) != 0) break; } //Comment: //CLOSE GRIP //Call Component Macro //Call Component Macro: Servo(0)::EnableServo(2) FCD_Servo0_EnableServo(2); //Calculation //Calculation: // servo1_Pos = 255 FCV_SERVO1_POS = 255; //Loop //Loop: While servo1_pos = 0 while (1) { //Calculation //Calculation: // servo1_Pos = servo1_Pos - 5 FCV_SERVO1_POS = FCV_SERVO1_POS - 5; //Call Component Macro //Call Component Macro: Servo(0)::SetPosition(2, servo1_Pos) FCD_Servo0_SetPosition(2, FCV_SERVO1_POS); //Delay //Delay: 100 ms delay_ms(100); if ((FCV_SERVO1_POS == 0) != 0) break; } //Call Component Macro //Call Component Macro: Servo(0)::DisableServo(2) FCD_Servo0_DisableServo(2); //Comment: //END GRIP //Comment: //UP BASE ARM //Calculation //Calculation: // servo1_Pos = 0 FCV_SERVO1_POS = 0; //Loop //Loop: While servo1_pos = 230 while (1) { //Calculation //Calculation: // servo1_Pos = servo1_Pos + 5 FCV_SERVO1_POS = FCV_SERVO1_POS + 5; //Call Component Macro //Call Component Macro: Servo(0)::SetPosition(1, servo1_Pos) FCD_Servo0_SetPosition(1, FCV_SERVO1_POS); //Delay //Delay: 100 ms delay_ms(100); if ((FCV_SERVO1_POS == 230) != 0) break; } //Comment: //OPEN GRIP //Calculation //Calculation: // servo1_Pos = 0 FCV_SERVO1_POS = 0; //Call Component Macro //Call Component Macro: Servo(0)::EnableServo(2) FCD_Servo0_EnableServo(2); //Loop //Loop: While servo1_pos = 255 while (1) { //Calculation //Calculation: // servo1_Pos = servo1_Pos + 5 FCV_SERVO1_POS = FCV_SERVO1_POS + 5; //Call Component Macro //Call Component Macro: Servo(0)::SetPosition(2, servo1_Pos) FCD_Servo0_SetPosition(2, FCV_SERVO1_POS); //Delay //Delay: 100 ms delay_ms(100); if ((FCV_SERVO1_POS == 255) != 0) break; } //Call Component Macro //Call Component Macro: Servo(0)::DisableServo(2) FCD_Servo0_DisableServo(2); //Delay //Delay: 600 ms delay_ms(255); delay_ms(255); delay_ms(90); //Comment: //UP JOINT //Calculation //Calculation: // servo1_Pos = 20 FCV_SERVO1_POS = 20; //Call Component Macro //Call Component Macro: Servo(0)::EnableServo(0) FCD_Servo0_EnableServo(0); //Loop //Loop: While servo1_pos = 0 while (1) { //Calculation //Calculation: // servo1_Pos = servo1_Pos - 5 FCV_SERVO1_POS = FCV_SERVO1_POS - 5; //Call Component Macro //Call Component Macro: Servo(0)::SetPosition(0, servo1_Pos) FCD_Servo0_SetPosition(0, FCV_SERVO1_POS); //Delay //Delay: 100 ms delay_ms(100); if ((FCV_SERVO1_POS == 0) != 0) break; } //Call Component Macro //Call Component Macro: Servo(0)::DisableServo(0) FCD_Servo0_DisableServo(0); //Comment: //CLOSE GRIP //Calculation //Calculation: // servo1_Pos = 255 FCV_SERVO1_POS = 255; //Call Component Macro //Call Component Macro: Servo(0)::EnableServo(2) FCD_Servo0_EnableServo(2); //Loop //Loop: While servo1_pos = 0 while (1) { //Calculation //Calculation: // servo1_Pos = servo1_Pos - 5 FCV_SERVO1_POS = FCV_SERVO1_POS - 5; //Call Component Macro //Call Component Macro: Servo(0)::SetPosition(2, servo1_Pos) FCD_Servo0_SetPosition(2, FCV_SERVO1_POS); //Delay //Delay: 100 ms delay_ms(100); if ((FCV_SERVO1_POS == 0) != 0) break; } //Call Component Macro //Call Component Macro: Servo(0)::DisableServo(2) FCD_Servo0_DisableServo(2); } void FCM_Sonar_Sense() { //Loop //Loop: While 1 while (1) { //Call Macro //Call Macro: RIGHT_SONAR FCM_RIGHT_SONAR(); //Call Macro //Call Macro: CENTER_SONAR FCM_CENTER_SONAR(); //Call Macro //Call Macro: LEFT_SONAR FCM_LEFT_SONAR(); //Comment: //TRY TO MOVE REVERSE //Comment: //TRY TO MOVE RIGHT //Decision //Decision: (echo2 < echo3) AND (echo2 < echo)? if ((FCV_ECHO2 < FCV_ECHO3) & (FCV_ECHO2 < FCV_ECHO)) { //Connection Point //Connection Point: [A]: A FCC_Sonar_Sense_A: ; //Decision //Decision: echo2 < 200? if (FCV_ECHO2 < 200) { //Decision //Decision: (echo2 < 120) AND (echo >100) AND (echo3 > 100)? if ((FCV_ECHO2 < 120) & (FCV_ECHO >100) & (FCV_ECHO3 > 100)) { //Output //Output: 0 -> PORTB trisb = 0x00; portb = 0; //Loop //Loop: While 1 while (1) { //Decision //Decision: echo2 > 110? if (FCV_ECHO2 > 110) { //Output //Output: 5 -> PORTB trisb = 0x00; portb = 5; //Delay //Delay: 50 ms delay_ms(50); //Output //Output: 0 -> PORTB trisb = 0x00; portb = 0; } else { //Decision //Decision: echo2 < 80? if (FCV_ECHO2 < 80) { //Output //Output: 10 -> PORTB trisb = 0x00; portb = 10; //Delay //Delay: 50 ms delay_ms(50); //Output //Output: 0 -> PORTB trisb = 0x00; portb = 0; } } //Call Macro //Call Macro: CENTER_SONAR FCM_CENTER_SONAR(); //Decision //Decision: echo2 < 110 AND echo2 > 80? if (FCV_ECHO2 < 110 & FCV_ECHO2 > 80) { //Output //Output: 0 -> PORTB trisb = 0x00; portb = 0; //Delay //Delay: 200 ms delay_ms(200); //Loop //Loop: Loop 20 times for (FCLV_LOOP1=0; FCLV_LOOP1<20; FCLV_LOOP1++) { //Input //Input: D7 -> height_sense trisd = trisd | 0x80; FCV_HEIGHT_SENSE = ((portd & 0x80) == 0x80); //Decision //Decision: height_sense = 0? if (FCV_HEIGHT_SENSE == 0) { //Output //Output: 10 -> PORTB trisb = 0x00; portb = 10; //Delay //Delay: 1 s delay_s(1); //Output //Output: 6 -> PORTB trisb = 0x00; portb = 6; //Delay //Delay: 2 s delay_s(2); //Goto Connection Point //Goto Connection Point: [D]: D goto FCC_Sonar_Sense_D; } //Delay //Delay: 20 ms delay_ms(20); } //Call Macro //Call Macro: duration FCM_duration(); //Goto Connection Point //Goto Connection Point: [D]: D goto FCC_Sonar_Sense_D; } } } else { //Output //Output: 5 -> PORTB trisb = 0x00; portb = 5; } //Connection Point //Connection Point: [D]: D FCC_Sonar_Sense_D: ; } else { //FIND_MOVE //Call Macro: FIND_MOVE FCM_FIND_MOVE(); } } else { //Comment: //CONFIRM IF THERE IS A BALL SOMEWHERE RIGHT //Decision //Decision: (echo < echo2) AND (echo < echo3)? if ((FCV_ECHO < FCV_ECHO2) & (FCV_ECHO < FCV_ECHO3)) { //Connection Point //Connection Point: [C]: C FCC_Sonar_Sense_C: ; //Decision //Decision: echo < 200? if (FCV_ECHO < 200) { //Decision //Decision: echo < 120? if (FCV_ECHO < 120) { //Output //Output: 10 -> PORTB trisb = 0x00; portb = 10; } else { //Loop //Loop: While 1 while (1) { //Output //Output: 9 -> PORTB trisb = 0x00; portb = 9; //Call Macro //Call Macro: RIGHT_SONAR FCM_RIGHT_SONAR(); //Call Macro //Call Macro: CENTER_SONAR FCM_CENTER_SONAR(); //Decision //Decision: echo > echo2? if (FCV_ECHO > FCV_ECHO2) { //Output //Output: 0 -> PORTB trisb = 0x00; portb = 0; //Goto Connection Point //Goto Connection Point: [A]: A goto FCC_Sonar_Sense_A; } } } } else { //Output //Output: 9 -> PORTB trisb = 0x00; portb = 9; //Comment: //DISTANCE IS TO MUCH, // JUST MOVE ON } } else { //Comment: //CONFIRM IF THERE IS A BALL SOMEWHERE LEFT //Decision //Decision: (echo3 < echo) AND (echo3 < echo2)? if ((FCV_ECHO3 < FCV_ECHO) & (FCV_ECHO3 < FCV_ECHO2)) { //Decision //Decision: echo3 < 200? if (FCV_ECHO3 < 200) { //Connection Point //Connection Point: [B]: B FCC_Sonar_Sense_B: ; //Decision //Decision: echo3 < 120? if (FCV_ECHO3 < 120) { //Output //Output: 10 -> PORTB trisb = 0x00; portb = 10; } else { //Loop //Loop: While 1 while (1) { //Output //Output: 6 -> PORTB trisb = 0x00; portb = 6; //Call Macro //Call Macro: LEFT_SONAR FCM_LEFT_SONAR(); //Call Macro //Call Macro: CENTER_SONAR FCM_CENTER_SONAR(); //Decision //Decision: echo3 > echo2? if (FCV_ECHO3 > FCV_ECHO2) { //Output //Output: 0 -> PORTB trisb = 0x00; portb = 0; //Goto Connection Point //Goto Connection Point: [A]: A goto FCC_Sonar_Sense_A; } } } } else { //Output //Output: 6 -> PORTB trisb = 0x00; portb = 6; //Comment: //DISTANCE IS TO MUCH, // JUST MOVE ON } } else { //FIND_MOVE //Call Macro: FIND_MOVE FCM_FIND_MOVE(); //Comment: //THERES NO BALL FIND OTHER pATH } } } } } void FCM_Movement() { //Call Component Macro //Call Component Macro: Servo(0)::DisableServo(2) FCD_Servo0_DisableServo(2); //Call Component Macro //Call Component Macro: Servo(0)::DisableServo(1) FCD_Servo0_DisableServo(1); //Call Component Macro //Call Component Macro: Servo(0)::DisableServo(0) FCD_Servo0_DisableServo(0); //Comment: //FORWARD //Loop //Loop: While 1 while (1) { //Loop //Loop: Loop 50 times for (FCLV_LOOP2=0; FCLV_LOOP2<50; FCLV_LOOP2++) { //Output //Output: 5 -> PORTB trisb = 0x00; portb = 5; //Delay //Delay: 100 ms delay_ms(100); } //Delay //Delay: 2 s delay_s(2); //Comment: //BACKWARD //Loop //Loop: Loop 50 times for (FCLV_LOOP3=0; FCLV_LOOP3<50; FCLV_LOOP3++) { //Output //Output: 10 -> PORTB trisb = 0x00; portb = 10; //Delay //Delay: 100 ms delay_ms(100); } //Delay //Delay: 2 s delay_s(2); //Comment: //LEFT //Loop //Loop: Loop 50 times for (FCLV_LOOP4=0; FCLV_LOOP4<50; FCLV_LOOP4++) { //Output //Output: 9 -> PORTB trisb = 0x00; portb = 9; //Delay //Delay: 100 ms delay_ms(100); } //Delay //Delay: 2 s delay_s(2); //Comment: //RIGHT //Loop //Loop: Loop 50 times for (FCLV_LOOP5=0; FCLV_LOOP5<50; FCLV_LOOP5++) { //Output //Output: 6 -> PORTB trisb = 0x00; portb = 6; //Delay //Delay: 100 ms delay_ms(100); } } } void FCM_RIGHT_SONAR() { //Comment: //RIGHT SONAR //Output //Output: 0 -> PORTC trisc = 0x00; portc = 0; //Calculation //Calculation: // echo = 0 FCV_ECHO = 0; //Loop //Loop: While in = 1 while (1) { //Output //Output: 1 -> PORTC trisc = 0x00; portc = 1; //Delay //Delay: 5 us delay_us(5); //Output //Output: 0 -> PORTC trisc = 0x00; portc = 0; //Input //Input: A0 -> in trisa = trisa | 0x01; FCV_IN = ((porta & 0x01) == 0x01); if ((FCV_IN == 1) != 0) break; } //Loop //Loop: While in = 0 while (1) { //Input //Input: A0 -> in trisa = trisa | 0x01; FCV_IN = ((porta & 0x01) == 0x01); //Calculation //Calculation: // echo = echo + 1 FCV_ECHO = FCV_ECHO + 1; //Delay //Delay: 2 us delay_us(2); if ((FCV_IN == 0) != 0) break; } //Delay //Delay: 30 ms delay_ms(30); } void FCM_CENTER_SONAR() { //Comment: //CENTER SONAR //Output //Output: 0 -> PORTC trisc = 0x00; portc = 0; //Calculation //Calculation: // echo2 = 0 FCV_ECHO2 = 0; //Loop //Loop: While in2 = 1 while (1) { //Output //Output: 2 -> PORTC trisc = 0x00; portc = 2; //Delay //Delay: 5 us delay_us(5); //Output //Output: 0 -> PORTC trisc = 0x00; portc = 0; //Input //Input: A1 -> in2 trisa = trisa | 0x02; FCV_IN2 = ((porta & 0x02) == 0x02); if ((FCV_IN2 == 1) != 0) break; } //Loop //Loop: While in2 = 0 while (1) { //Input //Input: A1 -> in2 trisa = trisa | 0x02; FCV_IN2 = ((porta & 0x02) == 0x02); //Calculation //Calculation: // echo2 = echo2 + 1 FCV_ECHO2 = FCV_ECHO2 + 1; //Delay //Delay: 2 us delay_us(2); if ((FCV_IN2 == 0) != 0) break; } //Delay //Delay: 30 ms delay_ms(30); } void FCM_LEFT_SONAR() { //Comment: //LEFT SONAR //Output //Output: 0 -> PORTC trisc = 0x00; portc = 0; //Calculation //Calculation: // echo3 = 0 FCV_ECHO3 = 0; //Loop //Loop: While in3 = 1 while (1) { //Output //Output: 4 -> PORTC trisc = 0x00; portc = 4; //Delay //Delay: 5 us delay_us(5); //Output //Output: 0 -> PORTC trisc = 0x00; portc = 0; //Input //Input: A2 -> in3 trisa = trisa | 0x04; FCV_IN3 = ((porta & 0x04) == 0x04); if ((FCV_IN3 == 1) != 0) break; } //Loop //Loop: While in3 = 0 while (1) { //Input //Input: A2 -> in3 trisa = trisa | 0x04; FCV_IN3 = ((porta & 0x04) == 0x04); //Calculation //Calculation: // echo3 = echo3 + 1 FCV_ECHO3 = FCV_ECHO3 + 1; //Delay //Delay: 2 us delay_us(2); if ((FCV_IN3 == 0) != 0) break; } //Delay //Delay: 30 ms delay_ms(30); } void FCM_FIND_MOVE() { //Decision //Decision: timer <= 10? if (FCV_TIMER <= 10) { //Output //Output: 5 -> PORTB trisb = 0x00; portb = 5; //Calculation //Calculation: // timer = timer + 1 FCV_TIMER = FCV_TIMER + 1; } else { //Decision //Decision: (timer > 10) AND timer <= 20? if ((FCV_TIMER > 10) & FCV_TIMER <= 20) { //Output //Output: 6 -> PORTB trisb = 0x00; portb = 6; //Calculation //Calculation: // timer = timer + 1 FCV_TIMER = FCV_TIMER + 1; } else { //Decision //Decision: (timer > 20) AND timer <= 30? if ((FCV_TIMER > 20) & FCV_TIMER <= 30) { //Output //Output: 10 -> PORTB trisb = 0x00; portb = 10; //Calculation //Calculation: // timer = timer + 1 FCV_TIMER = FCV_TIMER + 1; } else { //Decision //Decision: (timer > 30) AND timer <= 40? if ((FCV_TIMER > 30) & FCV_TIMER <= 40) { //Output //Output: 6 -> PORTB trisb = 0x00; portb = 6; //Calculation //Calculation: // timer = timer + 1 FCV_TIMER = FCV_TIMER + 1; } else { //Decision //Decision: (timer > 40) AND timer <= 50? if ((FCV_TIMER > 40) & FCV_TIMER <= 50) { //Output //Output: 5 -> PORTB trisb = 0x00; portb = 5; //Calculation //Calculation: // timer = timer + 1 FCV_TIMER = FCV_TIMER + 1; } else { //Decision //Decision: (timer > 50) AND timer <= 60? if ((FCV_TIMER > 50) & FCV_TIMER <= 60) { //Output //Output: 9 -> PORTB trisb = 0x00; portb = 9; //Calculation //Calculation: // timer = 0 FCV_TIMER = 0; } } } } } } } void FCM_X_CENTERING() { //Loop //Loop: While echo2 > 120 while (1) { //Output //Output: 9 -> PORTB trisb = 0x00; portb = 9; //Call Macro //Call Macro: CENTER_SONAR FCM_CENTER_SONAR(); if ((FCV_ECHO2 > 120) != 0) break; } //Output //Output: 0 -> PORTB trisb = 0x00; portb = 0; //Output //Output: 6 -> PORTB trisb = 0x00; portb = 6; //Delay //Delay: 200 ms delay_ms(200); //Output //Output: 0 -> PORTB trisb = 0x00; portb = 0; } void FCM_GET_BALL() { } void main() { //Initialisation adcon1 = 0x0F; ucfg = 0x08; cr_bit(pie2, USBIE); #ifdef MX_MIAC_SYSTEM #if (48 != MX_MIAC_MM_MIAC_BASE) #define MOD_ID ((0 << 5) + 48) << 9 MIAC_CAN_TX[0] = MX_SERVO_PIN_ARRAY[0]; MIAC_CAN_TX[1] = MX_SERVO_PIN_ARRAY[1]; MIAC_CAN_TX[2] = MX_SERVO_PIN_ARRAY[2]; MIAC_CAN_TX[3] = MX_SERVO_PIN_ARRAY[3]; MIAC_CAN_TX[4] = MX_SERVO_PIN_ARRAY[4]; MIAC_CAN_TX[5] = MX_SERVO_PIN_ARRAY[5]; MIAC_Module_WR(MOD_ID, 640, 6,1,1); MIAC_Module_GetAck(MOD_ID + 32, 640, 20); MIAC_CAN_TX[0] = MX_SERVO_PIN_ARRAY[6]; MIAC_CAN_TX[1] = MX_SERVO_PIN_ARRAY[7]; MIAC_CAN_TX[2] = MX_SERVO_TRIM[0]; MIAC_CAN_TX[3] = MX_SERVO_TRIM[1]; MIAC_CAN_TX[4] = MX_SERVO_TRIM[2]; MIAC_CAN_TX[5] = MX_SERVO_TRIM[3]; MIAC_Module_WR(MOD_ID, 640, 6,0,1); MIAC_Module_GetAck(MOD_ID + 32, 640, 20); MIAC_CAN_TX[0] = MX_SERVO_TRIM[4]; MIAC_CAN_TX[1] = MX_SERVO_TRIM[5]; MIAC_CAN_TX[2] = MX_SERVO_TRIM[6]; MIAC_CAN_TX[3] = MX_SERVO_TRIM[7]; MIAC_CAN_TX[4] = MX_SERVO_NO_CHAN; MIAC_Module_WR(MOD_ID, 640, 5,0,0); MIAC_Module_GetAck(MOD_ID + 32, 640, 20); #undef MOD_ID #endif #else char temp; ccpr1h = MX_SERVO_CCPR1H; //Set capture compare setpoint ccpr1l = MX_SERVO_CCPR1L; t1con = MX_SERVO_T1CON; //Setup timer and start running ccp1con = MX_SERVO_CCP1CON; //Configure CCP1 settings st_bit(pie1, CCP1IE); //Enable CCP1 interrupt ccp2con = MX_SERVO_CCP2CON; //Configure CCP2 settings st_bit(pie2, CCP2IE); //Enable CCP2 interrupt st_bit(intcon, PEIE); //Enable Interrupts st_bit(intcon, GIE); for (temp = 0; temp < MX_SERVO_NO_CHAN; temp++) { FCD_Servo0_ToggleTris(MX_SERVO_PORT_ARRAY[temp], MX_SERVO_PIN_ARRAY[temp], 1); //Convert Servo pin to input to disable motor } #endif //Interrupt initialisation code //Calculation //Calculation: // timer = 0 // status = 0 FCV_TIMER = 0; FCV_STATUS = 0; //Call Component Macro //Call Component Macro: Servo(0)::EnableServo(1) FCD_Servo0_EnableServo(1); //Call Component Macro //Call Component Macro: Servo(0)::SetPosition(1, 235) FCD_Servo0_SetPosition(1, 235); //Call Component Macro //Call Component Macro: Servo(0)::EnableServo(0) FCD_Servo0_EnableServo(0); //Call Component Macro //Call Component Macro: Servo(0)::SetPosition(0, 0) FCD_Servo0_SetPosition(0, 0); //Call Macro //Call Macro: Sonar_Sense FCM_Sonar_Sense(); mainendloop: goto mainendloop; } void MX_INTERRUPT_MACRO(void) { #ifndef MX_MIAC_SYSTEM if (ts_bit(pir1, CCP1IF) && ts_bit(pie1, CCP1IE)) { if (MX_SERVO_IDX < MX_SERVO_NO_CHAN) { FCD_Servo0_TogglePort(MX_SERVO_PORT_ARRAY[MX_SERVO_IDX], MX_SERVO_PIN_ARRAY[MX_SERVO_IDX], 1); //Start Servo duty cycle MX_SERVO_CALC = MX_SERVO_TRIM[MX_SERVO_IDX] + MX_SERVO_POSITION[MX_SERVO_IDX]; //Calculate the required position MX_SERVO_CALC = MX_SERVO_CALC * MX_SERVO_CLK_COUNT; //Calculate the number of clock cycles required MX_SERVO_CALC = (MX_SERVO_CALC >> 8) & 0xFFFF; //Shift back to be 16-bit ccpr2h = ( MX_SERVO_CALC >> 8 ); //Setup capture compare registers ccpr2l = ( MX_SERVO_CALC & 0xFF ); } cr_bit(pir1, CCP1IF); //Clear interrupt } if (ts_bit(pir2, CCP2IF) && ts_bit(pie2, CCP2IE)) { if (MX_SERVO_IDX < MX_SERVO_NO_CHAN) //If channel is active { FCD_Servo0_TogglePort(MX_SERVO_PORT_ARRAY[MX_SERVO_IDX], MX_SERVO_PIN_ARRAY[MX_SERVO_IDX],0); //End of control pulse } MX_SERVO_IDX = (MX_SERVO_IDX + 1) & 0x07; //Increment channel if (MX_SERVO_REQUIRED[MX_SERVO_IDX] > MX_SERVO_POSITION[MX_SERVO_IDX]) MX_SERVO_POSITION[MX_SERVO_IDX] = MX_SERVO_POSITION[MX_SERVO_IDX] + 1; //Increment duty if (MX_SERVO_REQUIRED[MX_SERVO_IDX] < MX_SERVO_POSITION[MX_SERVO_IDX]) MX_SERVO_POSITION[MX_SERVO_IDX] = MX_SERVO_POSITION[MX_SERVO_IDX] - 1; //Decrement duty cr_bit(pir2, CCP2IF); //Clear interrupt } #endif }