/*******************************************************************************
* FUNCTION NAME: Process_Data_From_Master_uP
* PURPOSE: Executes every 26.2ms when it gets new data from the master
* microprocessor.
* CALLED FROM: main.c
* ARGUMENTS: none
* RETURNS: void
*******************************************************************************/
void Process_Data_From_Master_uP(void)
{
static unsigned int j = 0;
Getdata(&rxdata);
Camera_Handler();
Servo_Track();
//PAN_SERVO = 127;
//TILT_SERVO = 127;
Default_Routine();
j++;
j++;
if(j == 1) {
printf("\rCalculating Gyro Bias...");
}
if(j == 6) {
Start_Gyro_Bias_Calc();
}
if(j == 200) {
Stop_Gyro_Bias_Calc();
Reset_Gyro_Angle();
printf("Done\r");
}
Putdata(&txdata);
}
/*******************************************************************************
* FUNCTION NAME: Process_Data_From_Master_uP
* PURPOSE: Executes every 26.2ms when it gets new data from the master
* microprocessor.
* CALLED FROM: main.c
* ARGUMENTS: none
* RETURNS: void
*******************************************************************************/
void Process_Data_From_Master_uP(void)
{
static unsigned char i;
Getdata(&rxdata); /* Get fresh data from the master microprocessor. */
if(disabled_mode)
{
Process_Disabled_Code();
}
else
{
Default_Routine(); /* Optional. See below. */
}
/* Add your own code here. (a printf will not be displayed when connected to the breaker panel unless a Y cable is used) */
//printf("shoulder_tar %3d, shoulder_pot %3d, shoulder_state %d\r",shoulder_tar,shoulder_pot,shoulder_state); /* printf EXAMPLE */
Generate_Pwms(pwm13,pwm14,pwm15,pwm16);
/* Example code to check if a breaker was ever tripped. */
Putdata(&txdata); /* DO NOT CHANGE! */
}
/*******************************************************************************
* FUNCTION NAME: User_Autonomous_Code
* PURPOSE: Execute user's code during autonomous robot operation.
* You should modify this routine by adding code which you wish to run in
* autonomous mode. It will be executed every program loop, and not
* wait for or use any data from the Operator Interface.
* CALLED FROM: main.c file, main() routine when in Autonomous mode
* ARGUMENTS: none
* RETURNS: void
*******************************************************************************/
void User_Autonomous_Code(void)
{
/* Initialize all PWMs and Relays when entering Autonomous mode, or else it
will be stuck with the last values mapped from the joysticks. Remember,
even when Disabled it is reading inputs from the Operator Interface.
*/
pwm01 = pwm02 = pwm03 = pwm04 = pwm05 = pwm06 = pwm07 = pwm08 = 127;
pwm09 = pwm10 = pwm11 = pwm12 = pwm13 = pwm14 = pwm15 = pwm16 = 127;
init_wings();
relay1_fwd = relay1_rev = relay2_fwd = relay2_rev = 0;
relay3_fwd = relay3_rev = relay4_fwd = relay4_rev = 0;
relay5_fwd = relay5_rev = relay6_fwd = relay6_rev = 0;
relay7_fwd = relay7_rev = relay8_fwd = relay8_rev = 0;
while (autonomous_mode) /* DO NOT CHANGE! */
{
if (statusflag.NEW_SPI_DATA) /* 26.2ms loop area */
{
Getdata(&rxdata); /* DO NOT DELETE, or you will be stuck here forever! */
/* Add your own autonomous code here. */
Process_Autonomous_Code();
Generate_Pwms(pwm13,pwm14,pwm15,pwm16);
Putdata(&txdata); /* DO NOT DELETE, or you will get no PWM outputs! */
}
}
}
/*******************************************************************************
* FUNCTION NAME: User_Autonomous_Code
* PURPOSE: Execute user's code during autonomous robot operation.
* You should modify this routine by adding code which you wish to run in
* autonomous mode. It will be executed every program loop, and not
* wait for or use any data from the Operator Interface.
* CALLED FROM: main.c file, main() routine when in Autonomous mode
* ARGUMENTS: none
* RETURNS: void
*******************************************************************************/
void User_Autonomous_Code(void)
{
static unsigned int Hybrid_Starting_Position=0;
static char Straight_Long_1=1;
static char Straight_Long_2=0;
static char Straight_Long_3=0;
static char Straight_Short_1=0;
static char Straight_Short_2=0;
static char Turn_90_1=0;
static char Turn_90_2=0;
static char Turn_90_3=0;
static char Turn_90_4=0;
static char Stop_0=0;
static char Stop_1=0;
static char Stop_2=0;
static char Stop_3=0;
static char Stop_4=0;
static int Loop=0;
/* Initialize all PWMs and Relays when entering Autonomous mode, or else it
will be stuck with the last values mapped from the joysticks. Remember,
even when Disabled it is reading inputs from the Operator Interface.
*/
pwm01 = pwm02 = pwm03 = pwm04 = pwm05 = pwm06 = pwm07 = pwm08 = 127;
pwm09 = pwm10 = pwm11 = pwm12 = pwm13 = pwm14 = pwm15 = pwm16 = 127;
relay1_fwd = relay1_rev = relay2_fwd = relay2_rev = 0;
relay3_fwd = relay3_rev = relay4_fwd = relay4_rev = 0;
relay5_fwd = relay5_rev = relay6_fwd = relay6_rev = 0;
relay7_fwd = relay7_rev = relay8_fwd = relay8_rev = 0;
Hybrid_Starting_Position=RC_Hybrid_Starting_Position_2;
Hybrid_Starting_Position=(Hybrid_Starting_Position << 1) | RC_Hybrid_Starting_Position_1;
RC_Pneum_Trans_High = 0;
RC_Pneum_Trans_Low = 1;
while (autonomous_mode) /* DO NOT CHANGE! */
{
if (statusflag.NEW_SPI_DATA) /* 26.2ms loop area */
{
Getdata(&rxdata); /* DO NOT DELETE, or you will be stuck here forever! */
/* Add your own autonomous code here. */
Generate_Pwms(pwm13,pwm14,pwm15,pwm16);
// if (RC_Hybrid_Move){
// if (Stop_0){
// if (Loop<=80){
// Auto_Out (127,127);
// Loop++;
// } else {
// Reset_Encoder_1_Count();
// Reset_Encoder_2_Count();
// Loop=0;
// Stop_0 = 0;
// Straight_Long_1 = 1;
// }
// }
if (Straight_Long_1){
if (Encoder_Right_Count<Bot_Straight_Long){
Auto_Out (60,187);
} else {
Reset_Encoder_1_Count();
Reset_Encoder_2_Count();
Straight_Long_1 = 0;
Stop_1 = 1;
}
}
if (Stop_1){
if (Loop<=25){
Auto_Out (117,135);
Loop++;
} else {
Reset_Encoder_1_Count();
Reset_Encoder_2_Count();
Loop=0;
Stop_1 = 0;
Turn_90_1 = 1;
}
}
if (Turn_90_1){
if (Encoder_Right_Count<Bot_Turn_90){
Auto_Out (127,255);
} else {
Reset_Encoder_1_Count();
Reset_Encoder_2_Count();
Turn_90_1 = 0;
Straight_Short_1 = 1;
}
}
if (Straight_Short_1){
if (Encoder_Right_Count<Bot_Straight_Short){
Auto_Out (60,187);
} else {
Reset_Encoder_1_Count();
Reset_Encoder_2_Count();
Stop_2 = 1;
Straight_Short_1 = 0;
}
}
if (Stop_2){
if (Loop<=25){
Auto_Out (117,135);
Loop++;
} else {
Reset_Encoder_1_Count();
Reset_Encoder_2_Count();
Loop=0;
Stop_2 = 0;
Turn_90_2 = 1;
}
}
if (Turn_90_2){
if (Encoder_Right_Count<Bot_Turn_90){
Auto_Out (70,187);
} else {
Reset_Encoder_1_Count();
Reset_Encoder_2_Count();
Turn_90_2 = 0;
Straight_Long_2 = 1;
}
}
if (Straight_Long_2){
if (Encoder_Right_Count<Bot_Straight_Long){
Auto_Out (60,187);
} else {
Reset_Encoder_1_Count();
Reset_Encoder_2_Count();
Straight_Long_2 = 0;
Stop_3 = 1;
}
}
/* if (Stop_3){
if (Loop<=20){
Auto_Out (127,127);
Loop++;
} else {
Reset_Encoder_1_Count();
Reset_Encoder_2_Count();
Loop=0;
Stop_3 = 0;
Turn_90_3 = 1;
}
}
if (Turn_90_3){
if (Encoder_Right_Count<Bot_Turn_90){
Auto_Out (127,187);
} else {
Reset_Encoder_1_Count();
Reset_Encoder_2_Count();
Turn_90_3 = 0;
Straight_Short_2 = 1;
}
}
if (Straight_Short_2){
if (Encoder_Right_Count<Bot_Straight_Short){
Auto_Out (60,187);
} else {
Reset_Encoder_1_Count();
Reset_Encoder_2_Count();
Stop_4 = 1;
Straight_Short_2 = 0;
}
}
if (Stop_4){
if (Loop<=20){
Auto_Out (127,127);
Loop++;
} else {
Reset_Encoder_1_Count();
Reset_Encoder_2_Count();
Loop=0;
Stop_4 = 0;
Turn_90_4 = 1;
}
}
if (Turn_90_4){
if (Encoder_Right_Count<Bot_Turn_90){
Auto_Out (127,187);
} else {
Reset_Encoder_1_Count();
Reset_Encoder_2_Count();
Turn_90_4 = 0;
Straight_Long_3 = 1;
}
}
if (Straight_Long_3){
if (Encoder_Right_Count<Bot_Straight_Long){
Auto_Out (60,187);
} else {
Reset_Encoder_1_Count();
Reset_Encoder_2_Count();
Straight_Long_3 = 0;
}
}
*/
// }
// if (RC_Hybrid_Move){// if (RC_Hybrid_Poke_1){// }// if (RC_Hybrid_Poke_2){// }
// RC_Hybrid_Dflt_Poke_Position 0=Inside; 1=Middle// Hybrid_Starting_Position 1=Inside ; 2=Middle ; 3=Outside; 0=Assume No Autonomous...if Hybrod_Move...will only drive straight }
//Process commands from IR Board//Button 1 Overpass 1 - 1st open position from inside//Button 2 Overpass 1 - 1st open position from outside//Button 3 Overpass 2 - 1st open position from inside//Button 4 Overpass 2 - 1st open position from outside// if (RC_IRBoard_CMD1){// }// if (RC_IRBoard_CMD2){// }// if (RC_IRBoard_CMD3){// }// if (RC_IRBoard_CMD4){// }
LabView_Out(); // Write Output to LabView or Data loggr
Putdata(&txdata); /* DO NOT DELETE, or you will get no PWM outputs! */
}
}
}
/*******************************************************************************
* FUNCTION NAME: User_Autonomous_Code
* PURPOSE: Execute user's code during autonomous robot operation.
* You should modify this routine by adding code which you wish to run in
* autonomous mode. It will be executed every program loop, and not
* wait for or use any data from the Operator Interface.
* CALLED FROM: main.c file, main() routine when in Autonomous mode
* ARGUMENTS: none
* RETURNS: void
*******************************************************************************/
void User_Autonomous_Code(void)
{
static int shooterPosition = 0; // 0 is down, 1 is up
static int shooterGoingDown = 0;
static int shooterGoingUp = 0;
static int shooterGoingUpCount = 0;
static int shooterGoingDownCount = 0;
static int readyToShoot = 0;
static int letMyAimBeTrue = 0;
static int inRange = 0;
int shooterOverride = 0;
static int loopCount = 0;
static int autoCount = 0;
int shooterButton = 0;
static int buttonCount = 0;
static int shooterRunning = 0;
static int shooterLoop = 0;
static int firstPush = 0;
static int firstLetGo = 0;
int initialDelay = 0;
int firstDrive = 500;
int firstTurn = 620;
int secondDrive = 1300;
int secondTurn = 0;
int thirdDrive = 0;
/* Initialize all PWMs and Relays when entering Autonomous mode, or else it
will be stuck with the last values mapped from the joysticks. Remember,
even when Disabled it is reading inputs from the Operator Interface.
*/
pwm01 = pwm02 = pwm03 = pwm04 = pwm05 = pwm06 = pwm07 = pwm08 = 127;
pwm09 = pwm10 = pwm11 = pwm12 = pwm13 = pwm14 = pwm15 = pwm16 = 127;
pwm11 = 26;
pwm12 = 254 - 26;
relay1_fwd = relay1_rev = relay2_fwd = relay2_rev = 0;
relay3_fwd = relay3_rev = relay4_fwd = relay4_rev = 0;
relay5_fwd = relay5_rev = relay6_fwd = relay6_rev = 0;
relay7_fwd = relay7_rev = relay8_fwd = relay8_rev = 0;
while (autonomous_mode) /* DO NOT CHANGE! */
{
if (statusflag.NEW_SPI_DATA) /* 26.2ms loop area */
{
Getdata(&rxdata); /* DO NOT DELETE, or you will be stuck here forever! */
/* Add your own autonomous code here. */
pwm05 = pwm06 = 254;
if ((shooterButton == 1) & (shooterRunning == 0))
{
shooterRunning = 1;
shooterLoop = 0;
buttonCount = 0;
}
if (shooterRunning == 1)
buttonCount++;
if ((buttonCount < 30) & (shooterRunning == 1))
{
shooterOverride = 1;
}
else if ((buttonCount < 50) & (shooterRunning == 1))
{
shooterOverride = 0;
}
else if ((buttonCount >= 65) & (shooterRunning == 1))
{
relay2_fwd = 0;
relay2_rev = 0;
buttonCount = 0;
shooterRunning = 0;
}
if (shooterOverride == 1)
{
firstLetGo = 1;
if (firstPush == 1)
{
loopCount = 0;
firstPush = 0;
}
if (loopCount < 45)
{
relay2_fwd = 0;
relay2_rev = 1;
loopCount++;
}
else
{
relay2_fwd = 0;
relay2_rev = 0;
}
}
else if (shooterOverride == 0)
{
if (firstLetGo == 1)
{
loopCount = 0;
firstLetGo = 0;
}
loopCount++;
firstPush = 1;
if (loopCount < 50)
{
relay2_fwd = 1;
relay2_rev = 0;
}
else
{
relay2_fwd = 0;
relay2_rev = 0;
}
}
Camera_Handler();
letMyAimBeTrue = Servo_Track(pwm01, pwm03);
if ((shooterGoingUp != 1) && (shooterGoingDown != 1) && (shooterPosition == 0) && (shooterOverride != 1) && (letMyAimBeTrue == 1))
readyToShoot = 1;
if ((letMyAimBeTrue > 13) && (letMyAimBeTrue < 40))
inRange = 1;
if ((letMyAimBeTrue != 1) && (readyToShoot == 1) )
{
readyToShoot = 0;
}
/*
// Motor Control
// Go backward
if (loopCount < 100)
{
pwm03 = pwm04 = 0;
pwm01 = pwm03 = 254;
}
else if (loopCount == 100)
pwm01 = pwm02 = pwm03 = pwm04 = 127;
// Left turn
else if (loopCount < 150)
{
pwm03 = pwm04 = 254;
pwm01 = pwm02 = 254;
}
// Go forward
else if (loopCount == 150)
pwm01 = pwm02 = pwm03 = pwm04 = 127;
else if (loopCount < 240)
{
pwm01 = pwm02 = 254;
pwm03 = pwm04 = 0;
}
// Stop
else
pwm01 = pwm02 = pwm03 = pwm04 = 127;
*/
if (autoCount < initialDelay)
pwm03 = pwm04 = pwm02 = pwm01 = 127;
else if (autoCount < firstDrive)
{
pwm03 = pwm04 = 0;
pwm01 = pwm02 = 254;
}
else if (autoCount < firstDrive + 5)
pwm03 = pwm04 = pwm02 = pwm01 = 127;
else if (autoCount < firstTurn)
{
pwm03 = pwm04 = 254;
pwm01 = pwm02 = 254;
}
else if (autoCount < firstTurn + 5)
pwm03 = pwm04 = pwm02 = pwm01 = 127;
else if (autoCount < secondDrive)
{
pwm03 = pwm04 = 0;
pwm01 = pwm02 = 254;
}
else
pwm03 = pwm04 = pwm01 = pwm02 = 127;
if ((autoCount > 600) & (autoCount % 150 < 10))
shooterButton = 1;
/*
if (loopCount > 200)
{
if (loopCount < 210)
shooterOverride = 1;
else if (loopCount < 300)
shooterOverride = 0;
else if (loopCount < 330)
shooterOverride = 1;
else if (loopCount < 390)
shooterOverride = 0;
else if (loopCount < 430)
shooterOverride = 1;
else if (loopCount < 490)
shooterOverride = 0;
else if (loopCount < 530)
shooterOverride = 1;
else if (loopCount < 590)
shooterOverride = 0;
else if (loopCount < 630)
shooterOverride = 1;
else if (loopCount < 690)
shooterOverride = 0;
else if (loopCount < 730)
shooterOverride = 1;
else if (loopCount < 790)
shooterOverride = 0;
else if (loopCount < 830)
shooterOverride = 1;
else if (loopCount < 890)
shooterOverride = 0;
else if (loopCount < 930)
shooterOverride = 1;
else if (loopCount < 990)
shooterOverride = 0;
else if (loopCount < 1030)
shooterOverride = 1;
else if (loopCount < 1090)
shooterOverride = 0;
*/
}
Generate_Pwms(pwm13,pwm14,pwm15,pwm16);
// ========================
// HOOD ANGLE
//=========================
if (pwm10 < 10)
{
pwm11 = 30;
pwm12 = 254 - pwm11;
}
else if (pwm10 < 15)
{
pwm11 = 39;
pwm12 = 254 - pwm11;
}
else if (pwm10 < 23)
{
pwm11 = 32;
pwm12 = 254 - pwm11;
}
else if (pwm10 < 55)
{
pwm11 = 26;
pwm12 = 254 - pwm11;
}
else if (pwm10 < 62)
{
pwm11 = 27;
pwm12 = 254 - pwm11;
}
else if (pwm10 < 65)
{
pwm11 = 30;
pwm12 = 254 - pwm11;
}
else
{
pwm11 = 26;
pwm12 = 254 - pwm11;
}
Putdata(&txdata); /* DO NOT DELETE, or you will get no PWM outputs! */
autoCount++;
}
}
/*******************************************************************************
* FUNCTION NAME: User_Initialization
* PURPOSE: This routine is called first (and only once) in the Main function.
* You may modify and add to this function.
* CALLED FROM: main.c
* ARGUMENTS: none
* RETURNS: void
*******************************************************************************/
void User_Initialization (void)
{
Set_Number_of_Analog_Channels(SIXTEEN_ANALOG); /* DO NOT CHANGE! */
/* FIRST: Set up the I/O pins you want to use as digital INPUTS. */
digital_io_01 = digital_io_02 = digital_io_03 = digital_io_04 = INPUT;
digital_io_05 = digital_io_06 = digital_io_07 = digital_io_08 = INPUT;
digital_io_09 = digital_io_10 = digital_io_11 = digital_io_12 = INPUT;
digital_io_13 = digital_io_14 = digital_io_15 = digital_io_16 = INPUT;
digital_io_18 = INPUT; /* Used for pneumatic pressure switch. */
/*
Note: digital_io_01 = digital_io_02 = ... digital_io_04 = INPUT;
is the same as the following:
digital_io_01 = INPUT;
digital_io_02 = INPUT;
...
digital_io_04 = INPUT;
*/
/* SECOND: Set up the I/O pins you want to use as digital OUTPUTS. */
digital_io_17 = INPUT; /* Example - Not used in Default Code. */
/* THIRD: Initialize the values on the digital outputs. */
// rc_dig_out17 = 0;
/* FOURTH: Set your initial PWM values. Neutral is 127. */
pwm01 = pwm02 = pwm03 = pwm04 = pwm05 = pwm06 = pwm07 = pwm08 = 127;
pwm09 = pwm10 = pwm11 = pwm12 = pwm13 = pwm14 = pwm15 = pwm16 = 127;
/* FIFTH: Set your PWM output types for PWM OUTPUTS 13-16.
/* Choose from these parameters for PWM 13-16 respectively: */
/* IFI_PWM - Standard IFI PWM output generated with Generate_Pwms(...) */
/* USER_CCP - User can use PWM pin as digital I/O or CCP pin. */
Setup_PWM_Output_Type(IFI_PWM,IFI_PWM,IFI_PWM,IFI_PWM);
/*
Example: The following would generate a 40KHz PWM with a 50% duty cycle on the CCP2 pin:
CCP2CON = 0x3C;
PR2 = 0xF9;
CCPR2L = 0x7F;
T2CON = 0;
T2CONbits.TMR2ON = 1;
Setup_PWM_Output_Type(USER_CCP,IFI_PWM,IFI_PWM,IFI_PWM);
*/
/* Add any other initialization code here. */
Init_Serial_Port_One();
Init_Serial_Port_Two();
#ifdef TERMINAL_SERIAL_PORT_1
stdout_serial_port = SERIAL_PORT_ONE;
#endif
#ifdef TERMINAL_SERIAL_PORT_2
stdout_serial_port = SERIAL_PORT_TWO;
#endif
Initialize_Encoders();
//I EXPECT INITIALIZATION VALUES
Reset_Encoder_1_Count(-153);
Reset_Encoder_2_Count(-415);
//start comment
Initialize_Gyro();
Initialize_ADC();
//end comment
init_pid(&arm, 100, 0, 0, 120, 35); // 275 0 0
init_pid(&wrist, 33, 0, 0, 20, 80); //45 30 0
init_pid(&Mr_Roboto, 55, 0 , 0, 100, 25);
init_pid(&robot_dist, 95, 0, 0, 100, 8);
init_pid(&gyro_c, 20, 0, 0, 100, 8);
Putdata(&txdata); /* DO NOT CHANGE! */
// *** IFI Code Starts Here ***
//
// Serial_Driver_Initialize();
printf("IFI 2006 User Processor Initialized ...\r"); /* Optional - Print initialization message. */
User_Proc_Is_Ready(); /* DO NOT CHANGE! - last line of User_Initialization */
}
void loop_2(void) {
Putdata(&txdata);
}