/*******************************************************************************
* 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: 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: 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! */
}
}
}
void main()
{
clrscr();
Getdata();
Sjf();
getch();
}
void loop_1(void)
{
Getdata(&rxdata);
#ifdef DEBUG
{
uint8_t i;
printf((char*)
"rxdata:\n"
" packet_num rcmode rcstatusflag: %i %i %i\n"
, rxdata.packet_num
, rxdata.rcmode.allbits
, rxdata.rcstatusflag.allbits);
_puts( " reserved_1[0..2] : ");
for(i = 0; i < 3; i++) {
printf((char*)"%i, ",rxdata.reserved_1[i]);
}
_puts( "; reserved_2[0..8] : ");
for(i = 0; i < 8; i++) {
printf((char*)"%i, ",rxdata.reserved_2[i]);
}
printf((char*) "\n"
" master_version : %i\n", rxdata.master_version);
printf((char*)
"statusflag: 0x%02x\n"
" ",statusflag.a);
for(i = 0; i < 8; i++) {
printf((char*)"%i, ", (statusflag.a & (1<<i) ) >> i);
}
_putc('\n');
}
#endif
}
/*******************************************************************************
* 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++;
}
}
void loop_1(void) {
Getdata(&rxdata);
}