 | Quote: #pragma config(Hubs, S1, HTServo, HTMotor, none, none)
#pragma config(Hubs, S2, HTMotor, none, none, none)
#pragma config(Hubs, S3, HTServo, none, none, none)
#pragma config(Sensor, S2, Magnet, sensorNone)
#pragma config(Sensor, S3, Sonar, sensorNone)
#pragma config(Sensor, S4, Magnet, sensorHiTechnicMagnetic)
#pragma config(Motor, mtr_S1_C2_1, Leftd, tmotorNormal, openLoop)
#pragma config(Motor, mtr_S1_C2_2, Rightd, tmotorNormal, openLoop, reversed)
#pragma config(Motor, mtr_S2_C1_1, lifter, tmotorNormal, openLoop)
#pragma config(Motor, mtr_S2_C1_2, sweeper, tmotorNormal, openLoop)
#pragma config(Servo, srvo_S1_C1_1, LiftServo1, tServoStandard)
#pragma config(Servo, srvo_S1_C1_2, MagnetElbow, tServoStandard)
#pragma config(Servo, srvo_S1_C1_3, MagnetSholder, tServoStandard)
#pragma config(Servo, srvo_S1_C1_4, MagnetServo, tServoStandard)
#pragma config(Servo, srvo_S1_C1_5, servo5, tServoNone)
#pragma config(Servo, srvo_S1_C1_6, StopServo, tServoStandard)
#pragma config(Servo, srvo_S3_C1_1, servo7, tServoNone)
#pragma config(Servo, srvo_S3_C1_2, servo8, tServoNone)
#pragma config(Servo, srvo_S3_C1_3, servo9, tServoNone)
#pragma config(Servo, srvo_S3_C1_4, servo10, tServoNone)
#pragma config(Servo, srvo_S3_C1_5, servo11, tServoNone)
#pragma config(Servo, srvo_S3_C1_6, LiftServo2, tServoStandard)
#pragma config(SrvoPosition, Position01, 131, 129, 129, 128, 128, 128, 128, 128)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
#include "JoystickDriver.c"
void initializeRobot()
{
// Place code here to initialize servos to starting positions.
// Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.
return;
}
task main()
{
initializeRobot();
int threshold = 20;
while (true)
{
getJoystickSettings(joystick);
if(abs(joystick.joy1_y2) > threshold) // If the right analog stick's Y-axis readings are either above or below the threshold:
{
motor[Rightd] = joystick.joy1_y2; // Right drive is assigned a power level equal to the right analog stick's Y-axis reading.
}
else // Else if the readings are within the threshold:
{
motor[Rightd] = 0; // Right drive is stopped with a power level of 0.
}
if(abs(joystick.joy1_y1) > threshold) // If the left analog stick's Y-axis readings are either above or below the threshold:
{
motor[Leftd] = joystick.joy1_y1; // Left drive is assigned a power level equal to the left analog stick's Y-axis reading.
}
else // Else if the readings are within the threshold:
{
motor[Leftd] = 0; // Leftdrive is stopped with a power level of 0.
}
{
if(SensorValue[Magnet] <640 )
servo[MagnetServo] = 127;
wait10Msec(250);
if (SensorValue[Magnet] >660 )
servo[MagnetServo] = 127;
wait10Msec(250);
if(SensorValue[Magnet] >=451)
{
servo[MagnetServo] = 255;
}
// First controller buttons below
if (joy1Btn(5)==1) //If Joy1-Button2 is pressed
{
servoTarget[LiftServo1] = 110; //lifts left side servo up to this position
servoTarget[LiftServo2] = 170; //lifts right side servo up to this position
}
else if (joy1Btn(7)==1) //If Joy1-Button3 is pressed
{
servoTarget[LiftServo1] = 20; //Turn arm on at -100% power
servoTarget[LiftServo2] = 210; //lifts right side servo down to this position
}
if (joy1Btn(4)==1)
{
motor[lifter] = -50;
}
else if (joy1Btn(1)==1)
{
motor[lifter] = 50;
}
else if (joy1Btn(2)==1)
{
motor[lifter] = 50;
wait10Msec(10);
}
else if (joy1Btn(3)==1)
{
motor[lifter]= -50;
wait10Msec(10);
}
else
{
motor[lifter] = 0;
}
// second controller entered below
// if (joy2Btn(5)) //If Joy2-Button 1 is pressed
{
//Open left arm at set value
}
if (joy2Btn(1)==1) //If Joy2-Button 1 is pressed
{
servoTarget[MagnetSholder]=34 //Swing the magnetic arm out at set value
;}
if (joy2Btn(4)==1) //If joy2-Button 4 is pressed
{
servoTarget[MagnetElbow]=210;
wait10Msec(30);
servoTarget[MagnetSholder]=165 //Bring the magnetic arm back to robot (at set value)
;}
if (joy2Btn(2)==1) //If joy2-Button 2 is pressed
{
servoTarget[MagnetElbow]=140 //Turn the magnet scoop half-way down (at set value)
// servoTarget[Bservo1] = 290; //
;}
else if (joy2Btn(3)==1) //If Joy2-Button 3 is pressed
{
servoTarget[MagnetElbow]=85; //Turn the magnet scoop all the way down (at set value)
//servoTarget[Bservo1] = 100;
}
if (joy2Btn(9)==1) //If Joy2-Button 9 is pressed
{
servoTarget[MagnetServo]=130; //The magnet flap will go up (to set value)
}
else if (joy2Btn(10)==1) //If Joy2-Button 10 is pressed
{
servoTarget[MagnetServo]=218; //The magnet flap will go back down (to set value)
}
if (joy2Btn(6)==1) //If Joy2-Button 6 is pressed
{
servoTarget[StopServo]= 30; //The flap stopping the balls goes up (to set value)
}
else if (joy2Btn(8)==1) //If Joy2-Button 8 is pressed
{
servoTarget[StopServo]= 180; //The flap stopping the balls swings back down (to set value)
}
if (joy2Btn(5)==1) //If Joy2Btn-5 is pressed
{
motor[sweeper] = 95;
}
else if (joy2Btn(7)==1)
{
motor[sweeper]= -95;
}
else
{
motor[sweeper]=0;
}
}
}
} | |
