Main Drive Code 2008
I cannot post the entire code due to copyright restructions. There is some scattered code in here that is not written by me, but was instead adapted from other places.
int Set_Neutral(int controller_input)
{
if((controller_input>=124)&&(controller_input<=131))
{
controller_input=127;
}
return controller_input;
}
/*******************************************************************************
* FUNCTION NAME: Set Neutral Close
* PURPOSE: Makes the controllers go to neutral if closeer to neutral then above
*
*
* ARGUMENTS: integer for what you are setting the PWM to
* RETURNS: The actual value to set
*******************************************************************************/
int Set_Neutral_Close(int controller_input)
{
if((controller_input>=126)&&(controller_input<=128))
{
controller_input=127;
}
return controller_input;
}
/*******************************************************************************
* FUNCTION NAME: Open Arm
* PURPOSE: Makes the arm open
*
* ARGUMENTS: none
* RETURNS: void
*******************************************************************************/
unsigned char OpenArmsDelay(void)
{
//You must set ArmOpener to 1 before calling
if(ArmOpener==1)
{
relay3_fwd = 1;
relay3_rev = 0;
ArmOpener++;
}
if(ArmOpener
ArmOpener++;
}
else if(ArmOpener==OPEN_DELAY)
{
relay5_fwd == 1;
relay5_rev == 0;
ArmOpener++;
}
if(ArmOpener>OPEN_DELAY)
{
relay3_fwd = 0;
relay5_fwd = 0;
ArmOpener = 0;
}
return ArmOpener;
}
/*******************************************************************************
* FUNCTION NAME: Default_Routine
* PURPOSE: Performs the default mappings of inputs to outputs for the
* Robot Controller.
* CALLED FROM: this file, Process_Data_From_Master_uP routine
* ARGUMENTS: none
* RETURNS: void
*******************************************************************************/
void Default_Routine(void)
{
/*********Mappings************************
("-" means its done 100%)
-p1_y = left side motor
-p2_y = Rght side motors
-p1_sw_trig = Slow left
-p2_sw_trig = Slow Right
-p3_sw_trig = Open Arms(with delay and punch)
-p3_sw_top = Close
-p3_sw_aux1 = Manual if 0, auto if 1
-p3_sw_aux2 - Top Placement
-p3_wheel = Slow both motors
-p3_x = Punch ball
-p3_y = Angle Pot
-Relay1_green = auto led
Relay1_red = pos1 led
Relay2_green = pos2 led
Relay2_red = pos3 led
Relay1 = Compressor
relay2 = Puncher
relay3 = One side open
relay4 = close both
relay5 = Open other side
******************************************/
//receive the arm angle
Arm_Angle = Get_Analog_Value(rc_ana_in02);
/****************Control box mappings************************/
//Standard Mapping
relay1_rev = !rc_dig_in16 || p1_sw_top;
relay4_fwd = p3_sw_top;
if(p3_sw_trig)
{
relay3_fwd = 1;
relay2_fwd=1;
RelayOpenDelay = 1;
if(Relay2OpenDelay<1)
{
Relay2OpenDelay=2;
}
//For Puncher (set this to only happen
//based on the POT value)
}
//Placement on top
else if(p3_sw_aux2)
{
relay3_fwd = 1;
relay5_fwd = 1;
}
else
{
relay3_fwd = 0;
relay5_fwd = 0;
}
//Manual Punch button
if(p3_x>200)
{
RelayOpenDelay=20;
}
//Automatic ball thing here
if(p3_sw_aux1)
{
if(usdistance>3200 && usdistance<7400)
{
relay4_fwd=1;
relay4_rev=0;
}
//Just so it doesnt stay open if aux 1 is still down
else
{
relay4_fwd=0;
relay4_rev=0;
}
}
/*****************Leds & Poteniometer********************/
if((usdistance2 <= (ULTRASONIC_LADDER + ULTRASONIC_DEADBAND) && (usdistance >= ULTRASONIC_LADDER - ULTRASONIC_DEADBAND))&&x==3)
{
//Relay1_green = 1;
//Relay1_red = 1;
Relay2_green = 1;
Relay2_red = 1;
}
else
{
//Relay1_green = 0;
//Relay1_red = 0;
Relay2_green = 0;
Relay2_red = 0;
}
// ****CHANGE THIS To the Pot thing once that is set up**
//POT_MAX = (x*ARM_MAX)+ARM_MIN
//So FACTOR_THING should be replaced with x
//x=(POT_MAX-ARM_MIN)/ARM_MAX
//pwm04 = p4_y; //This is backup just incase...
//pwm04 = Set_Neutral(Basic_Arm_Move( Arm_Angle, (FACTOR_THING*p3_y) + ARM_MIN) );
/****************************Delay Functions******************/
//Open Delayer
if(Relay2OpenDelay>1)
{
Relay2OpenDelay++;
}
if(Relay2OpenDelay==(OPEN_DELAY+1))
{
relay5_fwd = 1;
Relay2OpenDelay = 0;
}
/*
//open delay
if(p2_sw_trig)
{
relay3_fwd = 1;
Relay2OpenDelay++;
if(Relay2OpenDelay==20)
{
relay5_fwd = 1;
}
}*/
//punch delay
if((RelayOpenDelay<50)&&(RelayOpenDelay>5))
{
relay2_fwd = 1;
}
else
{
relay2_fwd=0;
}
if(RelayOpenDelay<60)
RelayOpenDelay++;
// printf("%d\r\n",RelayOpenDelay);
/**********************POT Position***************************
if(p4_y>240)
{
pwm04 = Basic_Arm_Move(Arm_Angle,530);
}
else if(p4_y<130 && p4_y>120)
{
pwm04 = Basic_Arm_Move(Arm_Angle,370);
}
else if(p4_y<30)
{
pwm04 = Basic_Arm_Move(Arm_Angle,100);
}
else
{
pwm04 = 127;
}
*/
if(p4_sw_trig)
{
pwm04 = Basic_Arm_Move(Arm_Angle,ARM_PICKUP);
}
else if(p4_sw_top)
{
pwm04 = Basic_Arm_Move3(Arm_Angle,ARM_SCOREING);
}
else if(p4_x>200)
{
pwm04 = Basic_Arm_Move(Arm_Angle,ARM_GETOFF);
/*
if(Arm_Angle>360 && pwm04>127)
{
pwm04-=30;
}*/
}
else
{
pwm04 = p4_y;
}
/*****************Joystick mapping******************************/
if(p1_sw_trig && p2_sw_trig)
{
Left_Motor = Set_Neutral(((56*(unsigned int)p1_y)>>7)+70);
Right_Motor = Set_Neutral(((56*(unsigned int)p1_y)>>7)+70);
}
else if(p1_sw_trig==1)
{
Left_Motor = Set_Neutral(((56*(unsigned int)p1_y)>>7)+70);
Right_Motor = Set_Neutral(p2_y);
}
else if(p2_sw_trig==1)
{
Left_Motor = Set_Neutral(p1_y);
Right_Motor = Set_Neutral(((56*(unsigned int)p1_y)>>7)+70);
}
else if(p3_wheel>200)
{
Left_Motor = Set_Neutral(((56*(unsigned int)p1_y)>>7)+70);
Right_Motor = Set_Neutral(((56*(unsigned int)p1_y)>>7)+70);
}
else
{
Left_Motor=Set_Neutral(p1_y);
Right_Motor=Set_Neutral(p2_y);
}
x++;
if(x==1)
{
//printf("Distance:%d\r\n",usdistance);
Ping_Start2();
}
if(x==4)
{
Ping_Start();
// printf("Distance2:%d\r\n",usdistance2);
//printf("Angle%d\r\n",Arm_Angle);
x=0;
}
//pwm04 = Basic_Arm_Move( Arm_Angle, (((unsigned int)(84*p4_y)/100)) + 40 ) ;
//printf("pwm04 %d, arm angle%d %d\r\n",pwm04,Arm_Angle,((int)(((p4_y*84)/100)) + ARM_MAX));
//printf("value = %d \r\n", ( ((unsigned int)(84*p4_y)/100)+40) );
/*********************Debugging loops**********************
x++;
if(x==1)
{
//temp_gyro = Get_Gyro_Angle();
//printf("%d\r\n",temp_gyro);//Get_Analog_Value(rc_ana_in01));
//printf("%d\r\n",temp_gyro_angle);
//Ping(3);
//Ping_Start();
}
if(x==20)
{
//printf("pwm01: %d\r\n",pwm01);
//printf("Distance:%d\r\n",Get_Encoder_Distance());
//printf("Distance:%d Time1:%d\r\n\r\n",Get_Distance(3),Ultrasonic_3_Time);
//printf("%d\r\n",PortTest1);
//printf(" %d%d ",Get_Encoder_1_Count());
//temp_gyro_angle = Get_Gyro_Angle();
//printf(" %d\r",lolhai);
//printf("%d%d\r\n\r\n",Get_Encoder_2_Count());
// printf("Distance: %d\r\n",usdistance);
//printf("%d\r\n",Get_X());
// printf("%d\r\n",Get_Y());
//printf("04 = %d 05 = %d 06 = %d 07 = %d\r",rc_dig_in04,rc_dig_in05,rc_dig_in06,rc_dig_in07);
printf("%d\r",Get_Analog_Value(rc_ana_in02));
//printf("%d\r",rc_ana_in01);
//printf("%d\r",Get_Analog_Value(rc_ana_in01));
//printf("%d\r",Get_Gyro_Angle());
//printf("%d\r",rc_dig_in01);
//printf("Input = 0: %d Input = 1: %d\r",two_test,one_test);
x=0;
//printf("%d\r",RelayOpenDelay);
//printf("X = %d Y = %d \r",Get_Analog_Value(rc_ana_in03),Get_Analog_Value(rc_ana_in04));
}*/
#include
#include "ifi_aliases.h"
#include "ifi_default.h"
#include "ifi_utilities.h"
#include "user_routines.h"
#include "user_Serialdrv.h"
#include "IR_Routines.h"
#include "Ultrasonic.h"
#include "ControlSystem.h"
#include "BasicControl.h"
#include "usdriver.h"
//gyro__find
#include "gyro.h"
#include "adc.h"
//long counter;
volatile long count;
volatile long count2;
unsigned int y;
int a;
volatile long timer_count;
extern long temp_gyro_angle;
extern volatile int phasetest;
extern volatile int phasetest2;
unsigned int auton_x;
long auton_y;
long auton_yaw;
unsigned int BallSeen;
int Arm_Angle2;
int uping;
unsigned int AutonInitialize;
unsigned char SwitchRoute;
extern unsigned char ArmOpener;
extern unsigned char IR_Routine_Number;
unsigned char old_portb;
unsigned char compare;
unsigned char int_byte;
if (INTCON3bits.INT2IF && INTCON3bits.INT2IE) /* The INT2 pin is RB2/DIG I/O 1. */
{
INTCON3bits.INT2IF = 0;
if(!rc_dig_in13)
{
count--;
}
else
{
count++;
}
}
else if (INTCON3bits.INT3IF && INTCON3bits.INT3IE) /* The INT3 pin is RB3/DIG I/O 2. */
{
INTCON3bits.INT3IF = 0;
if(!rc_dig_in12)
{
count2--;
}
else
{
count2++;
}
}
// else if (INTCONbits.RBIF && INTCONbits.RBIE) /* DIG I/O 3-6 (RB4, RB5, RB6, or RB7) changed. */
// {
// int_byte = PORTB; /* You must read or write to PORTB before the next step*/
// INTCONbits.RBIF = 0; /* and clear the interrupt flag */
// Ultrasonic_Interrupt_Routine(int_byte);
//This XOR operator ( ^ ) compares the old reading of the
//port B pin and will have a 1 in the bit that has changed
/*
compare = int_byte ^ old_portb;
old_portb = int_byte;
//PORTB bits are as follows:
//7=RB7 6=RB6 5=RB5 4=RB4 etc
//if bit falls on RB4 (dig_in03)
//We are going to shift over 4 so we dont get
//anything from RB3-RB0 And this also makes it get every
//other input I guess?
//rc_dig_in03
if(compare & 0x10)
{
if(int_byte & 0x10)
{
if(!rc_dig_in13)
count--;
else
count++;
}
}
//rc_dig_in04
if(compare & 0x20)
{
if(int_byte & 0x20)
{
if(!rc_dig_in12)
count2--;
else
count2++;
}
}
//rc_dig_in05
if(compare & 0x40)
{
}
//rc_dig_in06
if(compare & 0x80)
{
}
*/
// }
long Get_Encoder_1_Count(void)
{
long counter;
INTCONbits.RBIE = 0;
counter = count;
INTCONbits.RBIE = 1;
return counter;
}
long Get_Encoder_2_Count(void)
{
long counter2;
INTCONbits.RBIE = 0;
counter2 = -count2;
INTCONbits.RBIE = 1;
return counter2;
}
void Reset_Encoder_1_Count(void)
{
INTCONbits.RBIE = 0;
count = 0;
INTCONbits.RBIE = 1;
}
void Reset_Encoder_2_Count(void)
{
INTCONbits.RBIE = 0;
count2=0;
INTCONbits.RBIE = 1;
}
auton_x=0;
//uping = 0;
BallSeen = 0;
while (autonomous_mode) /* DO NOT CHANGE! */
{
Process_Data_From_Local_IO();
if (statusflag.NEW_SPI_DATA) /* 26.2ms loop area */
{
// printf("udistance:%d\r\n",usdistance);
Getdata(&rxdata); /* DO NOT DELETE, or you will be stuck here forever! */
if(uping == 1)
{
Ping_Start();
}
else if(uping == 4)
{
Ping_Start2();
uping=0;
}
uping++;
if(AutonInitialize==1)
{
pwm04 = 20;
}
else if(AutonInitialize==20)
{
pwm04=127;
}
if(AutonInitialize<=20)
{
AutonInitialize++;
}
if((usdistance2 <= (ULTRASONIC_LADDER + ULTRASONIC_DEADBAND) && (usdistance >= ULTRASONIC_LADDER - ULTRASONIC_DEADBAND)))
{
BallSeen = 0;
}
else
{
BallSeen = 0;
}
BallSeen = 0;
if(usdistance<20000)
{
pwm01=pwm02=127;
}
else
{
IR_All_Routines();
}
/*
printf("Encoder 1: %d Encoder 2: %d",(int)Get_Encoder_1_Count(),(int)Get_Encoder_2_Count());
Arm_Angle2 = Get_Analog_Value(rc_ana_in02);
if(AutonInitialize == 0)
{
//if we see the ball
if((usdistance2 <= (ULTRASONIC_LADDER + ULTRASONIC_DEADBAND) && (usdistance2 >= ULTRASONIC_LADDER - ULTRASONIC_DEADBAND)))
{
pwm01=pwm02=100;
AutonInitialize++;
}
else if(usdistance>3200 && usdistance<10000)
{
pwm01=pwm02=127;
}
else
{
pwm01 = Basic_Move((int)Get_Encoder_1_Count(),3000);
pwm02 = Basic_Move((int)Get_Encoder_2_Count(),3000);
if(Set_Neutral(pwm01) == 127 && Set_Neutral(pwm02) == 127)
{
AutonInitialize++;
}
}
}
else if(AutonInitialize == 1)
{
pwm01=pwm02=127;
AutonInitialize++;
}
else if(AutonInitialize == 2)
{
pwm01=pwm02=127;
pwm04 = Basic_Arm_Move3(Arm_Angle2,ARM_SCOREING);
if(Set_Neutral(pwm04) == 127)
{
AutonInitialize++;
}
}
else if(AutonInitialize ==3)
{
pwm01=pwm02=127;
pwm04 = Basic_Arm_Move(Arm_Angle2,ARM_PICKUP);
if(Set_Neutral(pwm04) == 127)
{
AutonInitialize++;
}
}
else if((AutonInitialize == 4) && (Arm_Angle2 > ARM_GETOFF))
{
pwm01 = Basic_Move((int)Get_Encoder_1_Count(),3000);
pwm02 = Basic_Move((int)Get_Encoder_2_Count(),3000);
}
else if((AutonInitialize == 4) && (Arm_Angle2 >= ARM_GETOFF))
{
Switch3_LED=1;
}
else
{
pwm01=pwm02=127;
}
/*
if(IR_PORT1)
{
auton_x =3;
AutonInitialize = 0;
}
else if(IR_PORT2)
{
auton_x = 0;
AutonInitialize = 1;
}
if((usdistance2 <= (ULTRASONIC_LADDER + ULTRASONIC_DEADBAND) && (usdistance >= ULTRASONIC_LADDER - ULTRASONIC_DEADBAND)))
{
if(!AutonInitialize)
{
pwm01=pwm02=127;
}
}
else if(usdistance>3200 && usdistance<10000)
{
pwm01=pwm02=127;
}
else if(auton_x == 0)
{
pwm01 = Basic_Move((int)Get_Encoder_1_Count(),1570);
pwm02 = Basic_Move((int)Get_Encoder_2_Count(),1570);
if(Set_Neutral(pwm01) == 127 && Set_Neutral(pwm02) == 127)
{
auton_x++;
}
}
else if(auton_x == 1)
{
pwm01 = pwm02 =127;
}
/****This is where we are going to
Turn 90 and reset encoders/gyro
AND we need to open the arms
and we need to test the switch*****/
//Turn 1570 milliradians
//auton_y = Get_Gyro_Angle();
/*
//Swtich one open means we turn + 90 degrees
if(SWITCH1)
{
SwitchRoute=0;
//1570 miliradians = 90 degrees
pwm01 = Set_Neutral(Basic_Move((int)auton_y,1570))*12 / 10;
pwm02 = 255 - pwm01;
if(pwm01 == 127)
{
//reset
Reset_Gyro_Angle();
Reset_Encoder_1_Count();
Reset_Encoder_2_Count();
AutonInitialize++;
}
}
//Swtich one closed means we turn - 90 degrees
if(!SWITCH1)
{
SwitchRoute=1;
//1570 miliradians = 90 degrees
pwm01 = Set_Neutral(Basic_Move((int)auton_y,1570));
if(pwm01 == 127)
{
AutonInitialize++;
}
}
}
else if(AutonInitialize==1)
{
//Move to position
if(SWITCH1)
{
}
if(!SWITCH1)
{
}
}
else if(AutonInitialize==2)
{
//Opening the arms
ArmOpener=1;
OpenArmsDelay();
if(ArmOpener==OPEN_DELAY)
{
Reset_Gyro_Angle();
Reset_Encoder_1_Count();
Reset_Encoder_2_Count();
AutonInitialize++;
if(SwitchRoute)
{
AutonInitialize=4;
}
}
}
else if(AutonInitialize==4)
{
//Ball Routine
}
else
{
IR_All_Routines(); /*edit these in IR_Routines.c*/
//IR_Routine_Number use this to determine the loc
/***Start automacitc field tracking around thing here
if(!auton_x)
{
Reset_Encoder_1_Count();
Reset_Encoder_2_Count();
auton_x++;
}
if((auton_x < 2) )
{
pwm01 = Set_Neutral(Basic_Move(Get_Encoder_1_Count(),1249));
pwm02 = Set_Neutral(Basic_Move(Get_Encoder_2_Count(),1249));
if(pwm01 == 127 && pwm02 == 127)
{
auton_x++;
}
}
if(auton_x == 2)
{
Reset_Encoder_1_Count();
Reset_Encoder_2_Count();
auton_x++;
auton_yaw = Get_Gyro_Angle();
printf("Angle: %d\r\n",(int)Get_Gyro_Angle());
}
if(auton_x == 3)
{
auton_y = Get_Gyro_Angle();
//3141 miliradians = 180 degrees
pwm01 = Set_Neutral(Basic_Move((int)auton_y,(int)auton_yaw + 3141))*12 / 10;
pwm02 = 255 - pwm01;
if(pwm01 == 127)
{
auton_x ++;
}
printf("Angle: %d\r\n",(int)auton_y);
}
if(auton_x >3)
{
pwm01=pwm02=127;
}
printf("num: %d\r\n",auton_x);*/
}
/*********************
Define our own aliases for easy changing. We have pwm01 =pwm02
and pwm03 is seperate as well as pwm04 for the arm
**********************/
//#define Right_Motor pwm02
//#define Left_Motor pwm01
#define Right_Motor pwm01
#define Left_Motor pwm02
#define SWITCH1 rc_dig_in13
#define SWITCH2 rc_dig_in14
#define OPEN_DELAY 6
#define ARM_MIN 548
#define ARM_MAX 40
#define ARM_POSITION1 548
#define ARM_POSITION2 300
#define ARM_POSITION3 370
//#define FACTOR_THING -3
#define FACTOR_THING 84 / 100
#define ARM_DEADBAND 5
#define POT_MIN 252
#define POT_MAX 0
#define OPEN_SERVO pwm09
#define ULTRASONIC_DEADBAND 500
#define ULTRASONIC_LADDER 12100
#define ULTRASONIC_DISTANCE 6400
#define ENCODER_FACTOR 4.36
/*************************
To get this you want to find
The amount of times it takes for the encoder wheel
to go around for every time the wheel goes around
from the 2007 robot it is about 2.5 so
EncoderWheelFactor = 2.5
Then get the Wheel Circumference, on 2007 robit its about 28.2735
WheelCircumference = 28.2735
Then the number of encoder ticks, on 2007 it is
EncoderTicks = 128
To find this factor we do
(EncoderWheelFactor * EncoderTicks) / WheelCircumference
2.5 * 128 = 320
320/28.2735 = 11.318018639361946699205970254832
new = 10.412577148212990963269492634446
So for every inch we have 11.318018639361946699205970254832 encoder tick
encoder ticks = Distance * encoder factor
So to find A distance, multiply distance * encoder factor = Encoder ticks
**************************/
* FUNCTION NAME: Set Neutral
* PURPOSE: Makes the controllers go to neutral if close to neutral*
*
* ARGUMENTS: integer for what you are setting the PWM to
* RETURNS: The actual value to set
*******************************************************************************/