two_motor_driver.c

#pragma config(Hubs,  S4, HTMotor,  HTServo,  HTMotor,  HTMotor)
#pragma config(Sensor, S1,     HTDIR,          sensorNone)
#pragma config(Sensor, S2,     TOUCH,          sensorNone)
#pragma config(Sensor, S3,     HTDIR,          sensorNone)
#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_C3_1,     motorF,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S4_C3_2,     motorG,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S4_C4_1,     motorH,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S4_C4_2,     motorI,        tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S4_C2_1,    servo1,               tServoStandard)
#pragma config(Servo,  srvo_S4_C2_2,    servo2,               tServoNone)
#pragma config(Servo,  srvo_S4_C2_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S4_C2_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S4_C2_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S4_C2_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

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

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()
{
  return;
}

task main()
{
  initializeRobot();

  waitForStart();

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

  float servostate = servo[servo1];
  float clawDivisor = 300;

  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);
    int center = cleanup(joystick.joy2_y1)/5;


    motor[motorD] = right;
    motor[motorE] = -left;
    motor[motorF] = center;
    motor[motorG] = -center;

    int s = cleanup (joystick.joy2_y2);

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