void armUpDead()
{
motorSet(ARM_MOTOR1, -MOTOR_MAX); // arm up
motorSet(ARM_MOTOR2, MOTOR_MAX);
motorSet(ARM_MOTOR3, -MOTOR_MAX);
motorSet(ARM_MOTOR4, MOTOR_MAX);
}
void UC_intake() {
if (joystickGetDigital(1, 6, JOY_UP)) {
motorSet(Intake, 127);
} else if (joystickGetDigital(1, 6, JOY_DOWN)) {
motorSet(Intake, -127);
} else {
motorSet(Intake, 0);
}
}
/** Set the Motor Speeds according to the SET_* Motor Position Constants.
* \param axis ROLL or PITCH
* \param vals Speeds for the two Motors on this axis (+, -)
*/
inline void setMotorSpeeds(uint8_t axis, uint8_t *vals) {
if (axis == ROLL) {
motorSet(SET_ROLLPLUS, vals[0]);
motorSet(SET_ROLLMINUS, vals[1]);
} else if (axis == PITCH) {
motorSet(SET_PITCHPLUS, vals[0]);
motorSet(SET_PITCHMINUS, vals[1]);
}
}
/* Runs a motor 0.5sec in each direction to make sure it's working */
void pulseMotor(tMotor motor) {
StopTask(usercontrol);
motorSet(motor,127);
wait1Msec(250);
motorSet(motor,-127);
wait1Msec(250);
motorSet(motor);
StartTask(usercontrol);
}
void turnRightDead ()
{
motorSet (motor1, -64) ;
motorSet (motor2, -64) ;
motorSet (motor10, 64) ;
motorSet (motor9, 64) ;
}
void driveForward(int encoder_value){
while((encoderGet(frontLeftEncoder)+ encoderGet(frontRightEncoder))/2 < encoder_value){
motorSet(front_left_drive, -127);
motorSet(front_right_drive, 127);
motorSet(back_left_drive, -127);
motorSet(back_right_drive, -127);
}
encoderReset(frontLeftEncoder);
encoderReset(frontRightEncoder);
}
void driveBackSlowDead()
{
motorSet (motor1, -40) ;
motorSet (motor2, -40) ;
motorSet (motor10, -40);
motorSet (motor9, -40) ;
}
void turnLeft(int encoder_value){
while(((-1*encoderGet(frontLeftEncoder)) + encoderGet(frontRightEncoder))/2 > encoder_value){
motorSet(front_left_drive, 127);
motorSet(front_right_drive, 127);
motorSet(back_left_drive, 127);
motorSet(back_right_drive, - 127);
}
encoderReset(frontLeftEncoder);
encoderReset(frontRightEncoder);
}
void outtake(int x) {
motorSet(MOTOR5, -127); // arm rightLine
motorSet(MOTOR6, 127); // arm leftLine
delay(x);
motorSet(MOTOR5, 0); // stop
motorSet(MOTOR6, 0); //
}
void outtake(int x) {
motorSet(MOTOR_ARM_RIGHT_MID, -127); // arm rightLine
motorSet(MOTOR_ARM_LEFT_MID, 127); // arm leftLine
delay(x);
motorSet(MOTOR_ARM_RIGHT_MID, 0); // stop
motorSet(MOTOR_ARM_LEFT_MID, 0); //
}
void driveForwardSlowDead()
{
motorSet (motor1, 40) ;
motorSet (motor2, 40) ;
motorSet (motor10, 40);
motorSet (motor9, 40) ;
}
void outtake(int x)
{
motorSet (5, -127) ;
motorSet (6, -127) ;
delay (x);
motorSet (5, 0) ;
motorSet (6, 0) ;
}
void intake(int x)
{
motorSet (5, 127) ;
motorSet (6, 127) ;
delay (x);
motorSet (5, 0) ;
motorSet (6, 0) ;
}
/*
Initializes PWM timers for ESC control
\param none
Motor 1: PB4 using TIMER1_A
Motor 2: PB5 using TIMER1_B
Motor 3: PB0 using TIMER2_A
Motor 4: PB1 using TIMER2_B
\return none
*/
void initPWM()
{
// Configure output pins
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB);
GPIOPinConfigure(GPIO_PB4_T1CCP0);
GPIOPinTypeTimer(GPIO_PORTB_BASE, GPIO_PIN_4);
GPIOPinConfigure(GPIO_PB5_T1CCP1);
GPIOPinTypeTimer(GPIO_PORTB_BASE, GPIO_PIN_5);
GPIOPinConfigure(GPIO_PB0_T2CCP0);
GPIOPinTypeTimer(GPIO_PORTB_BASE, GPIO_PIN_0);
GPIOPinConfigure(GPIO_PB1_T2CCP1);
GPIOPinTypeTimer(GPIO_PORTB_BASE, GPIO_PIN_1);
// Configure the two timers as half width (16bit) pwm (though we get 24 bits with the "prescale")
SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER1);
TimerConfigure(TIMER1_BASE, TIMER_CFG_SPLIT_PAIR|TIMER_CFG_A_PWM|TIMER_CFG_B_PWM);
TimerControlLevel(TIMER1_BASE, TIMER_BOTH, true);
SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER2);
TimerConfigure(TIMER2_BASE, TIMER_CFG_SPLIT_PAIR|TIMER_CFG_A_PWM|TIMER_CFG_B_PWM);
TimerControlLevel(TIMER2_BASE, TIMER_BOTH, true);
//calculate pulse repetition rate
uint32_t cycles_per_interval = SysCtlClockGet() / MOTORUPDATE /*Hz*/;
uint32_t periodl = cycles_per_interval;
uint8_t periodh = periodl >> 16;
periodl &= 0xFFFF;
//set pulse repetition rate
TimerPrescaleSet(TIMER1_BASE, TIMER_A, periodh);
TimerLoadSet(TIMER1_BASE, TIMER_A, periodl);
TimerPrescaleSet(TIMER1_BASE, TIMER_B, periodh);
TimerLoadSet(TIMER1_BASE, TIMER_B, periodl);
TimerPrescaleSet(TIMER2_BASE, TIMER_A, periodh);
TimerLoadSet(TIMER2_BASE, TIMER_A, periodl);
TimerPrescaleSet(TIMER2_BASE, TIMER_B, periodh);
TimerLoadSet(TIMER2_BASE, TIMER_B, periodl);
//set all motors to zero
motorSet(1,0);
motorSet(2,0);
motorSet(3,0);
motorSet(4,0);
//enable timers
TimerEnable(TIMER1_BASE, TIMER_A);
TimerEnable(TIMER1_BASE, TIMER_B);
TimerEnable(TIMER2_BASE, TIMER_A);
TimerEnable(TIMER2_BASE, TIMER_B);
}
void armUpTime(int millis)
{
motorSet(ARM_MOTOR3, MOTOR_MAX); // arm up
motorSet(ARM_MOTOR4, MOTOR_MAX);
motorSet(ARM_MOTOR7, MOTOR_MAX);
motorSet(ARM_MOTOR8, MOTOR_MAX);
delay (millis);
armUpTrim();
}
void intake (int x)
{
motorSet (motor5, 127) ;// arm rightLine
motorSet (motor6, -127) ;// arm leftLine
delay(x);
motorSet (motor5, 0) ;// stop
motorSet (motor6, 0) ;//
}
void armDown(int x) {
int pot = analogRead(8);
while (pot > x) {
motorSet(MOTOR3, 127); // arm down
motorSet(MOTOR4, -127);
motorSet(MOTOR7, 127);
motorSet(MOTOR8, -127);
pot = analogRead(8);
}
armDownTrim();
}
void armDownTime(int millis)
{
motorSet(ARM_MOTOR3, -MOTOR_MAX); // arm Down
motorSet(ARM_MOTOR4, -MOTOR_MAX);
motorSet(ARM_MOTOR7, -MOTOR_MAX);
motorSet(ARM_MOTOR8, -MOTOR_MAX);
delay (millis);
armDownTrim();
}
void operatorControl() {
while (true) {
if (digitalRead(11) == 0) {
motorSet(1, 127);
motorSet(10, 127);
}
if (digitalRead(11) == 1) {
motorSet(1, 0);
motorSet(10, 0);
}
}
}
void armDownTime(int x)
{
motorSet (motor3, 127) ; // arm Down
motorSet (motor4, -127) ;
motorSet (motor7, 127) ;
motorSet (motor8, -127) ;
delay (x);
armDownTrim();
}
void armUpTime(int x)
{
motorSet (motor3, -127) ; // arm up
motorSet (motor4, 127) ;
motorSet (motor7, -127) ;
motorSet (motor8, 127) ;
delay (x);
armUpTrim();
}
void driveBack(int x) {
int counts = 0;
imeReset(0); // rest rightLine I.M.E
imeGet(0, &counts);
while (abs(counts) < x) {
motorSet(MOTOR1, REVERSE_VELOCITY);
motorSet(MOTOR2, REVERSE_VELOCITY);
motorSet(MOTOR10, REVERSE_VELOCITY);
motorSet(MOTOR9, REVERSE_VELOCITY);
imeGet(0, &counts); // keep getting the value
}
motorSet(MOTOR1, INERTIA_CANCELLATION_FACTOR); // no inertia
motorSet(MOTOR2, INERTIA_CANCELLATION_FACTOR);
motorSet(MOTOR10, INERTIA_CANCELLATION_FACTOR);
motorSet(MOTOR9, INERTIA_CANCELLATION_FACTOR);
delay(65);
motorStop(MOTOR1);
motorStop(MOTOR2);
motorStop(MOTOR10);
motorStop(MOTOR9);
delay(200);
}
void turnLeft(int x)
{
int counts = 0;
imeReset(0); // rest rightLine ime
imeGet(0, &counts);
while (abs(counts) < x)
{
motorSet (motor1, 64) ;
motorSet (motor2, 64) ;
motorSet (motor10, -64) ;
motorSet (motor9, -64) ;
imeGet(0, &counts); // keep getting the value
}
motorSet (motor1, -10) ; // no inertia
motorSet (motor2, -10) ;
motorSet (motor10, 10) ;
motorSet (motor9, 10) ;
delay (45);
motorStop (motor1);
motorStop (motor2);
motorStop (motor10);
motorStop (motor9);
delay (200);
}
void driveBack(int x)
{
int counts = 0;
imeReset(0); // rest rightLine ime
imeGet(0, &counts);
while (abs(counts) < x)
{
motorSet (MOTOR_DRIVE_RIGHT_LINE_BACK, REVERSE_DRIVE_SPEED) ;
motorSet (MOTOR_DRIVE_RIGHT_LINE_FRONT, REVERSE_DRIVE_SPEED) ;
motorSet (MOTOR_DRIVE_LEFT_LINE_BACK, REVERSE_DRIVE_SPEED) ;
motorSet (MOTOR_DRIVE_LEFT_LINE_FRONT, REVERSE_DRIVE_SPEED) ;
imeGet(0, &counts); // keep getting the value
}
motorSet (MOTOR_DRIVE_RIGHT_LINE_BACK, 7) ; // no inertia
motorSet (MOTOR_DRIVE_RIGHT_LINE_FRONT, 7) ;
motorSet (MOTOR_DRIVE_LEFT_LINE_BACK, 7) ;
motorSet (MOTOR_DRIVE_LEFT_LINE_FRONT, 7) ;
delay (65);
motorStop (MOTOR_DRIVE_RIGHT_LINE_BACK);
motorStop (MOTOR_DRIVE_RIGHT_LINE_FRONT);
motorStop (MOTOR_DRIVE_LEFT_LINE_BACK);
motorStop (MOTOR_DRIVE_LEFT_LINE_FRONT);
delay (200);
}
void driveBack(int x)
{
int counts = 0;
imeReset(0); // rest rightLine ime
imeGet(0, &counts);
while (abs(counts) < x)
{
motorSet (motor1, -100) ;
motorSet (motor2, -100) ;
motorSet (motor10, -100) ;
motorSet (motor9, -100) ;
imeGet(0, &counts); // keep getting the value
}
motorSet (motor1, 7) ; // no inertia
motorSet (motor2, 7) ;
motorSet (motor10, 7) ;
motorSet (motor9, 7) ;
delay (65);
motorStop (motor1);
motorStop (motor2);
motorStop (motor10);
motorStop (motor9);
delay (200);
}
void operatorControl() {
lcdInitiate();
while (true){
ch3 = joystickAxes(3);
ch1 = joystickAxes(1);
drive(ch3, ch1);
// lift code
lift();
// pincer code
pincers();
lcd();
delay(100);
if(joystickGetDigital(2,7,JOY_DOWN)){
motorSet(left_lift_Motor1, -127);
delay(2000);
motorSet(left_lift_Motor1, 0);
motorSet(left_lift_Motor2, 127);
delay(2000);
motorSet(left_lift_Motor2, 0);
motorSet(right_lift_Motor3, -127);
delay(2000);
motorSet(right_lift_Motor3, 0);
motorSet(right_lift_Motor4, 127);
delay(2000);
motorSet(right_lift_Motor4, 0);
}
}
}
void turnLeft(int x)
{
int counts = 0;
imeReset(0); // rest rightLine ime
imeGet(0, &counts);
while (abs(counts) < x)
{
motorSet (MOTOR_DRIVE_RIGHT_LINE_BACK, MOTOR_TURN_SPEED) ;
motorSet (MOTOR_DRIVE_RIGHT_LINE_FRONT, MOTOR_TURN_SPEED) ;
motorSet (MOTOR_DRIVE_LEFT_LINE_BACK, -MOTOR_TURN_SPEED) ;
motorSet (MOTOR_DRIVE_LEFT_LINE_FRONT, -MOTOR_TURN_SPEED) ;
imeGet(0, &counts); // keep getting the value
}
motorSet (MOTOR_DRIVE_RIGHT_LINE_BACK, -10) ; // no inertia
motorSet (MOTOR_DRIVE_RIGHT_LINE_FRONT, -10) ;
motorSet (MOTOR_DRIVE_LEFT_LINE_BACK, 10) ;
motorSet (MOTOR_DRIVE_LEFT_LINE_FRONT, 10) ;
delay (45);
motorStop (MOTOR_DRIVE_RIGHT_LINE_BACK);
motorStop (MOTOR_DRIVE_RIGHT_LINE_FRONT);
motorStop (MOTOR_DRIVE_LEFT_LINE_BACK);
motorStop (MOTOR_DRIVE_LEFT_LINE_FRONT);
delay (200);
}
void armUp(int x) {
int pot = analogRead(8);
while (pot < x) {
motorSet(MOTOR3, -127); // arm up
motorSet(MOTOR4, 127);
motorSet(MOTOR7, -127);
motorSet(MOTOR8, 127);
pot = analogRead(8);
}
armUpTrim();
delay(300);
}
void turnRight(int x) {
int counts = 0;
imeReset(0); // rest rightLine I.M.E
imeGet(0, &counts);
while (abs(counts) < x) {
motorSet(MOTOR1, -64);
motorSet(MOTOR2, -64);
motorSet(MOTOR10, 64);
motorSet(MOTOR9, 64);
imeGet(0, &counts); // keep getting the value
}
motorSet(MOTOR1, 10); // no inertia
motorSet(MOTOR2, 10);
motorSet(MOTOR10, -10);
motorSet(MOTOR9, -10);
delay(45);
motorStop(MOTOR1);
motorStop(MOTOR2);
motorStop(MOTOR10);
motorStop(MOTOR9);
delay(200);
}
void liftUp(int x){
/*if(analogRead(lift_pot < 4000)){// if the lift is vertical
motorSet(pincer_right, 127);// open the pincers
motorSet(pincer_left, -127);
motorSet(left_lift_Motor1, 0);// stop the lift
motorSet(left_lift_Motor2, 0);
motorSet(right_lift_Motor3, 0);
motorSet(right_lift_Motor4, 0);
}*/
motorSet(left_lift_Motor1, -127);
motorSet(left_lift_Motor2, -127);
motorSet(right_lift_Motor3, 127);
motorSet(right_lift_Motor4, 127);
delay(x);
}