 | Code: #pragma config(Hubs, S1, HTMotor, HTMotor, HTMotor, HTServo) #pragma config(Sensor, S1, , sensorI2CMuxController) #pragma config(Sensor, S3, lightSensor, sensorLightInactive) #pragma config(Motor, motorA, , tmotorNXT, openLoop) #pragma config(Motor, motorB, , tmotorNXT, openLoop) #pragma config(Motor, motorC, , tmotorNXT, openLoop) #pragma config(Motor, mtr_S1_C1_1, LeftDrive, tmotorTetrix, openLoop, encoder) #pragma config(Motor, mtr_S1_C1_2, RightDrive, tmotorTetrix, openLoop, encoder) #pragma config(Motor, mtr_S1_C2_1, Base1, tmotorTetrix, openLoop) #pragma config(Motor, mtr_S1_C2_2, Base2, tmotorTetrix, openLoop) #pragma config(Motor, mtr_S1_C3_1, Top, tmotorTetrix, openLoop) #pragma config(Motor, mtr_S1_C3_2, motorI, tmotorTetrix, openLoop) #pragma config(Servo, srvo_S1_C4_1, LWrist, tServoStandard) #pragma config(Servo, srvo_S1_C4_2, RWrist, tServoStandard) #pragma config(Servo, srvo_S1_C4_3, Ramp, tServoStandard) #pragma config(Servo, srvo_S1_C4_4, Stop, tServoStandard) #pragma config(Servo, srvo_S1_C4_5, AutoWrist, tServoStandard) #pragma config(Servo, srvo_S1_C4_6, AutoArm, tServoStandard) //*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
#include "JoystickDriver.c"
void initializeRobot() {
servoTarget[LWrist] = 230; servoTarget[RWrist] = 0; servoTarget[AutoArm] = 0; servoTarget[AutoWrist] = 120; servoTarget[Stop] = 0; servoTarget[Ramp] = 245; wait1Msec(200); }
task main() { initializeRobot();
waitForStart(); { servo[AutoArm] = 140; wait1Msec(900); }{ servo[AutoArm] = 70; wait1Msec(900); }{ servo[AutoWrist] = 0; wait1Msec(900); }{ servo[AutoArm] = 10; wait1Msec(500); }
nMotorEncoder[LeftDrive] = 0; nMotorEncoder[RightDrive] = 0;
while (true) { if ((nMotorEncoder[LeftDrive] + nMotorEncoder[RightDrive])/2 < 500) { motor[LeftDrive] = -50; motor[RightDrive] = 50; } else { motor[LeftDrive] = 0; motor[RightDrive] = 0; } }
} |  |