void updateSwerveModule(SwerveModule &swerveModule)
{
int reading = HTSPBreadADC(proto, swerveModule.number, 8);
int turnSpeed = calcPID(swerveModule.turnPID, reading);
if ( swerveModule.inverted )
turnSpeed = -turnSpeed;
servo[swerveModule.turnMotor] = -(turnSpeed+(swerveModule.turnZeroOffset+127));
nxtDisplayString(5, "%i", -(turnSpeed+(swerveModule.turnZeroOffset+127)));
/*
if ( abs(swerveModule.driveSpeed) > 10 )
motor[swerveModule.driveMotor] = (swerveModule.driveMotorInverted?-swerveModule.driveSpeed:swerveModule.driveSpeed);
else if ( turnSpeed > 10 )
motor[swerveModule.driveMotor] = -13;
else if ( turnSpeed < -10 )
motor[swerveModule.driveMotor] = 13;
else
motor[swerveModule.driveMotor] = 0;
*/
}
task main() {
int inputdata;
ubyte outputdata;
int bit;
// Set B0 as output
HTSPBsetupIO(HTSPB, 0x1);
while(true) {
// Read a 10bit wide analogue value from A0
inputdata = HTSPBreadADC(HTSPB, 2, 10);
float wrist = (inputdata - rMin);
wrist /= (rMax - rMin);
servo [wristMain] = 255 * wrist;
nxtDisplayTextLine(1, "A2: %d", inputdata);
nxtDisplayTextLine(3, "W: %1.2f", wrist);
// Set the output bit based on the analogue input value
bit = (inputdata/128);
if (bit > 5) bit = 5;
outputdata = 1 << bit;
HTSPBwriteIO(HTSPB, outputdata);
wait1Msec(50);
}
}
bool hasTube(){
bool have=true;
if(HTSPBreadADC(HTPB,2,10)>=600){
have=true;
}
else{
have=false;
// if(HTSPBreadIO(HTPB,haveTube)==0)have=false;
}
return have;
}
void initSwerveModule(int number, tMotor driveMotor, TServoIndex turnMotor, bool inverted, int turnZeroOffset)
{
initPID(modules[number].turnPID, 0.01, 0, 0);
modules[number].turnPID.target = 200;
modules[number].turnMotor = turnMotor;
modules[number].driveMotor = driveMotor;
modules[number].number = number;
modules[number].inverted = inverted;
modules[number].turnZeroOffset = turnZeroOffset;
int reading = HTSPBreadADC(proto, number, 8);
}
task main() {
short inputdata;
while(true) {
// Read a 10bit wide analogue value from A0
inputdata = HTSPBreadADC(HTSPB, 0, 10);
eraseDisplay();
displayTextLine(1, "Light: %d", inputdata);
sleep(50);
}
}
void rotateRightLP(ubyte amount, int pw){
setStrobe(HTPB,turnR | amount);
io=io|turning;
writeIO();
motor[left]=pw;
motor[right]=pw;
while(HTSPBreadADC(HTPB,almost,10)<600);
motor[left]=20;
motor[right]=20;
while(!HTSPBreadIO(HTPB,doneTurning));
drive(0);
io=io& ~turning;
io=io|doneTurning;
writeIO();
}
void armBaseUpdate()
{
int potInput = HTSPBreadADC(S3, 0, 10)+BASE_OFFSET;
static int loopsStable = 0;
if ( abs(base.error) <= 60 ) // was 75
{
if ( loopsStable < 1000 ) loopsStable++;
}
else
loopsStable = 0;
if ( potInput < 500 && potInput > 270 )
{
// Arm is in top range - very little force needed to maintain position
base.Kp = 0.7;
base.Ki = 0.01;
base.errorSum = 0;
}
else
{
base.Kp = 1.5;
base.Ki = 0.07;
}
if ( abs(base.error) > 70 && base.target == BASE_PICKUP ) // Arm is headed forward and we are far away
base.Kd = 2.5; // increase D to prevent slaming against the floor
//else if ( potInput > 450 && base.target > 450 ) // TODO: check if this D modification is still needed
// base.Kd = 2.5;
else
base.Kd = 0.4;
if ( loopsStable <= 9 ) // was 20
motor[armBase] = dbc(limitVar(calcPID(base, potInput), 80), 23);
else
{
calcPID(base, potInput); // keep PID updated for error calculation
// even though we are not using it's output
base.errorSum = 0; // prevent I from building up
motor[armBase] = 0;
}
}
task main() {
int inputdata;
// Set B0 for output
HTSPBsetupIO(HTSPB, 0x1);
while(true) {
// Read a 10bit wide analogue value from A0
inputdata = HTSPBreadADC(HTSPB, 0, 10);
nxtDisplayTextLine(1, "A0: %d", inputdata);
// If A0 is less than 50% of the max value
// turn off the LED, otherwise switch it on
if(inputdata < 512)
HTSPBwriteIO(HTSPB, 0x00);
else
HTSPBwriteIO(HTSPB, 0x01);
wait1Msec(50);
}
}
task main() {
HTSPBsetupIO(HTSPB, 0x10);
while(true) {
// Read the value from the temp sensor
inputdata = HTSPBreadADC(HTSPB, 0, 10);
// Convert to an actual temperature
temperatureC = ((inputdata*3300)/1023-600)/10.0;
nxtDisplayTextLine(1, "Temp: %d C", temperatureC);
// If we're above 28 degrees, switch on the LED
if(temperatureC > THRESHOLD) {
HTSPBwriteIO(HTSPB, 0x10);
} else {
HTSPBwriteIO(HTSPB, 0x00);
}
wait1Msec(50);
}
}
task main() {
int inputdata;
ubyte outputdata;
int bit;
// Set B0 as output
HTSPBsetupIO(HTSPB, 0xFF);
while(true) {
// Read a 10bit wide analogue value from A0
inputdata = HTSPBreadADC(HTSPB, 0, 10);
displayTextLine(1, "A0: %d", inputdata);
// Set the output bit based on the analogue input value
bit = (inputdata/128);
if (bit > 5) bit = 5;
outputdata = 1 << bit;
HTSPBwriteIO(HTSPB, outputdata);
wait1Msec(50);
}
}
task main() {
short inputdata;
ubyte outputdata;
short bit;
// Set all digital IOs as outputs as output
HTSPBsetupIO(HTSPB, 0xFF);
while(true) {
// Read a 10bit wide analogue value from A0
inputdata = HTSPBreadADC(HTSPB, 0, 10);
displayTextLine(1, "A0: %d", inputdata);
// Set the output bit based on the analogue input value
bit = (inputdata/128);
if (bit > 5) bit = 5;
displayTextLine(2, "Bit: %d", bit);
outputdata = 1 << bit;
HTSPBwriteIO(HTSPB, outputdata);
sleep(50);
}
}
task main()
{
//Holonomic Drive Integers
int x1 = 0;
int y1 = 0;
int x2 = 0;
const int t = 8;
const float driveScale = 0.7;
initializeRobot ();
while ( true ) {
getJoystickSettings(joystick);
//Holonomic Drive
x1 = ( abs( joystick.joy1_x1 ) > t ) ? joystick.joy1_x1 : 0;
y1 = ( abs( joystick.joy1_y1 ) > t ) ? joystick.joy1_y1 : 0;
x2 = ( abs( joystick.joy1_x2 ) > t ) ? joystick.joy1_x2 : 0;
motor[frontLeft] = (y1 + x2 + x1) * driveScale;
motor[frontRight] = (y1 - x2 - x1) * driveScale;
motor[rearLeft] = (y1 + x2 - x1) * driveScale;
motor[rearRight] = (y1 - x2 + x1) * driveScale;
//This is the code for controlling the motorized linear slides that the ring grabber appparatus is mounted on.
//Up
if ((joy2Btn (5) == 1) && (SensorValue [liftUpSensor] == 0))
{
motor [linearSlides] = 100;
}
//Down; after the button statement is the sensor value statement so the lift doesn't go too far down.
else if ((joy2Btn (7) == 1) && (SensorValue [liftDownSensor] == 0 )) {
motor [linearSlides] = -100;
}
else motor [linearSlides] = 0;
//The following is the code for controlling the ring grabber apparatus.
//This is the code for controlling the arm that the grabber is mounted on.
if (joy2Btn (6) == 1 ) {
motor [grabberMount] = 50;
}
else if (joy2Btn (8) == 1) {
motor [grabberMount] = -50;
}
else motor [grabberMount] = 0;
/* The following is the other arm code; this allows the arm to be controlled with the joysticks (in addition to
the buttons. */
//motor [grabberMount] = (-(abs( joystick.joy2_y2 ) > t ) ? joystick.joy2_y2 : 0 );
//This is the code for controlling the grabber itself. It is 'commented' out because we currently do not use the 'active' grabber.
/*
if (joystick.joy2_x2 >110)
/*(joy2Btn (1) == 1)*/
/* if (grabberTarget >0)
{
grabberTarget -= grabberIncrement;
setGrabber (grabberTarget/100.0);
}
if (joystick.joy2_x2 <-110)
/*(joy2Btn (3) == 1)*/
/* if (grabberTarget <100)
{
grabberTarget += grabberIncrement;
setGrabber (grabberTarget/100.0);
} */
//This is the code for controlling the wrist, which the grabber is attatched to.
#if 0
if (joystick.joy2_y1 >110)
/*(joy2Btn (2) == 1)*/
if (wristTarget >0)
{
wristTarget -= wristIncrement;
setWrist (wristTarget/1000.0);
}
if (joystick.joy2_y1 <-110)
/*(joy2Btn (4) == 1)*/
if (wristTarget <1000)
{
wristTarget += wristIncrement;
setWrist (wristTarget/1000.0);
}
#else
const int rMax = 1023;
const int rMin = 407;
int inputdata = HTSPBreadADC(HTSPB, 2, 10);
float wrist = (inputdata - rMin);
wrist /= (rMax - rMin);
servo [wristMain] = (255 * wrist) + ( joystick.joy2_y1 / 3);
#endif
// LED weight sensor lights
#if 1
int inputRed = HTSPBreadADC(HTSPB, 0, 10);
int inputBlue = HTSPBreadADC(HTSPB, 1, 10);
int output = 0x00;
if ( inputRed > 512 )
output |= 0x01;
if ( inputBlue > 512 )
output |= 0x02;
if ( output != lastOutput ) {
HTSPBwriteLED( HTSPB, output );
HTSPBwriteIO( HTSPB, output );
lastOutput = output;
}
//wait1Msec(50);
#endif
/* Perhaps we will switch to this function system at the next meeting or possible time. Uncomment below,
the 'deployRamp' function above, and the latchSetting varible declaration. */
/* if ((joy1Btn (10) == 1) && (joy2Btn (10) == 1))
{
deployRamp (180);
} */
/*This conditional statement releases the ramp latch for the end game. Both 'start' buttons must be
pressed to release the ramp latch. The button combination that must be pressed is intentionally awkward
so that the ramp is not deployed by accident during the first 90 seconds of the teleop phase. */
/* Also, below there is (commented out) the or operator and an additional button combination in which
both drivers press all four colored buttons on the right side of the joysticks to deploy the ramp;
this is not currently implemented because it has not been tested and proven to work. */
if /*(*/((joy1Btn (10) == 1) && (joy2Btn (10) == 1)) /* || ((joy1Btn (1) == 1) && (joy1Btn (2) == 1) &&
(joy1Btn (3) == 1) && (joy1Btn (4) == 1) && (joy2Btn (1) == 1) && (joy2Btn (2) == 1) &&
(joy2Btn (3) == 1) && (joy2Btn (4) == 1))) */
{
servo [rampLatch] = rampLatchOpen;
}
}
}
task Arm()
{
int count = 0;
float lightVal = 0;
float lightAvg = 0;
bool bucketFull = false;
bool counted = false;
while(true)
{
getJoystickSettings(joystick);
if (joy1Btn(BTN_LB) && !joy1Btn(BTN_LT))
motor[lift] = 75;
else if (joy1Btn(BTN_LT) && !joy1Btn(BTN_LB))
motor[lift] = -75;
else
motor[lift] = 0;
if (joy1Btn(BTN_RB) && !joy1Btn(BTN_RT) && runIntake)
motor[intake] = 100;
else if (joy1Btn(BTN_RT) && !joy1Btn(BTN_RB) && runIntake)
motor[intake] = -100;
else
motor[intake] = 0;
lightVal = HTSPBreadADC(htspb, 0, 10);
//writeDebugStreamLine("Count: %d", count);
//writeDebugStreamLine("Avg: %d", lightAvg);
//writeDebugStreamLine("Val: %d", lightVal);
if (lightVal > (lightAvg - 40) && !counted && time1[T3] > 1200)
{
count += 1;
counted = true;
writeDebugStreamLine("Count: %d", count);
//writeDebugStreamLine("Avg: %d", lightAvg);
//writeDebugStreamLine("Val: %d", lightVal);
ClearTimer(T3);
}
else if (counted && lightAvg < (lightVal + 15))
{
counted = false;
}
else if (time1[T2] > 500)
{
lightAvg = lightVal;
ClearTimer(T2);
}
if (count == 5)
{
//bucketFull = true;
ClearTimer(T1);
count = 0;
}
if (time10[T1] > 300 && bucketFull)
{
runIntake = false;
bucketFull = false;
}
if (TSreadState(touch) == 1 && !closeOverride)
servo[inputstop] = 130;
else if (TSreadState(touch) == 0 && !closeOverride)
servo[inputstop] = 235;
if (time1[T4] > 5000)
closeOverride = false;
}
}
