two_motor_driver 2.0.c

#pragma config(Hubs,  S4, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S1,     HTDIR,          sensorNone)
#pragma config(Sensor, S2,     TOUCH,          sensorNone)
#pragma config(Sensor, S3,     HTDIR,          sensorNone)
#pragma config(Sensor, S4,     ,               sensorI2CMuxController)
#pragma config(Motor,  mtr_S4_C1_1,     motorD,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S4_C1_2,     motorE,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S4_C2_1,     motorF,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S4_C2_2,     motorG,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S4_C3_1,     motorH,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S4_C3_2,     motorI,        tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S4_C4_1,    servo1,               tServoStandard)
#pragma config(Servo,  srvo_S4_C4_2,    servo2,               tServoStandard)
#pragma config(Servo,  srvo_S4_C4_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S4_C4_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S4_C4_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S4_C4_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.
#include "axel_rose_constants.c"


int cleanup(int x) {
  const int threshold = 10;
  if (x > threshold)
    return (x - threshold) * (x - threshold) /  128;
  if (x < -threshold)
    return (x + threshold) * (x + threshold) / -128;
  return 0;
}

void initializeRobot()
{
	servo[servo2] = AUTO_ARM_STARTING_POSITION;
	servo[servo1] = CLAW_STARTING_POSITION;
  return;
}

task main()
{
  initializeRobot();

  waitForStart();

  nxtDisplayString(0, "Zip tie time!...");

  float servostate = servo[servo1];
  float clawDivisor = 300;
  bool armUp = false;
  bool buttonPressed = false;

  while (true)
  {
    getJoystickSettings(joystick);
		nxtDisplayString(1, "x:%d y:%d", joystick.joy1_x1, joystick.joy1_y1);
		nxtDisplayString(2, "x:%d y:%d", joystick.joy1_x2, joystick.joy1_y2);

    int left   = cleanup(joystick.joy1_y1);
    int right = cleanup(joystick.joy1_y2);
		nxtDisplayString(3, "left:%d", left);
		nxtDisplayString(4, "right:%d", right);
		nxtDisplayString(5, "s:%d", servo[servo1]);
    int center = cleanup(joystick.joy2_y1);
    if (center > 0) {
    	center = (int)((float)center * 0.4);
    } else {
      if (joystick.joy2_Buttons & 64) {
      	center = (int)((float)center * 0.4);
      } else {
        center = (int)((float)center * .15);
      }
    }

    motor[motorD] = -left;
    motor[motorE] = right;
    motor[motorH] = center;
    motor[motorI] = -center;


    int winch_up = joystick.joy2_Buttons & 16;
    int winch_down = joystick.joy2_Buttons & 32;


    if (winch_up && !winch_down){
    	motor[motorF] = 75;
    	motor[motorG] = -75;
  	}
  	else if (!winch_up && winch_down){
  		motor[motorF] = -75;
    	motor[motorG] = 75;
  	}
  	else {
  		motor[motorF] = 0;
    	motor[motorG] = 0;
  	}
		int spinnerOn = joystick.joy1_Buttons & 4;
		int spinnerReverse = joystick.joy1_Buttons & 8;

		if (spinnerOn > 0) {
			motor[motorA] = 255;
		} else if (spinnerReverse > 0) {
			motor[motorA] = -255;
		} else {
			motor[motorA] = 0;
		}
		if ((joystick.joy1_Buttons & 256)>0)  {
			if (!buttonPressed){
			buttonPressed = true;
			armUp = !armUp;
		}
}
		else{
			buttonPressed = false;
}
		if (armUp) {
			servo[servo2] = 120;
}
	else{
		servo[servo2] = 0;
}

    int pickupPosition = joystick.joy2_Buttons & 2;
    if (pickupPosition > 0) {
    	servostate = 200;
    }

    int s = cleanup (joystick.joy2_y2);

    float new_servostate = servostate - s / clawDivisor;
    if (new_servostate < 20){
      new_servostate = 20;
    }
    if (new_servostate > 255){
      new_servostate = 255;
    }
    servostate = new_servostate;
    servo[servo1] = (int) servostate;
    bFloatDuringInactiveMotorPWM = false;
  }
}