 | Code: #pragma config(I2C_Usage, I2C1, i2cSensors) #pragma config(Sensor, in1, Gripperpotent, sensorPotentiometer) #pragma config(Sensor, dgtl5, encbl, sensorQuadEncoder) #pragma config(Sensor, dgtl7, encbr, sensorQuadEncoder) #pragma config(Sensor, dgtl9, encfl, sensorQuadEncoder) #pragma config(Sensor, dgtl11, encfr, sensorQuadEncoder) #pragma config(Sensor, I2C_1, , sensorQuadEncoderOnI2CPort, , AutoAssign ) #pragma config(Sensor, I2C_2, , sensorQuadEncoderOnI2CPort, , AutoAssign ) #pragma config(Sensor, I2C_3, , sensorQuadEncoderOnI2CPort, , AutoAssign ) #pragma config(Sensor, I2C_4, , sensorQuadEncoderOnI2CPort, , AutoAssign ) #pragma config(Motor, port1, backLeft, tmotorVex393HighSpeed_HBridge, PIDControl, reversed, encoderPort, dgtl5) #pragma config(Motor, port2, frontLeft, tmotorVex393HighSpeed_MC29, PIDControl, encoderPort, dgtl9) #pragma config(Motor, port3, lifttopright, tmotorVex393HighSpeed_MC29, PIDControl, encoderPort, I2C_2) #pragma config(Motor, port4, liftbottomright, tmotorVex393HighSpeed_MC29, PIDControl, encoderPort, I2C_1) #pragma config(Motor, port5, grippermotor, tmotorVex393HighSpeed_MC29, openLoop) #pragma config(Motor, port7, lifttopleft, tmotorVex393HighSpeed_MC29, PIDControl, reversed, encoderPort, I2C_3) #pragma config(Motor, port8, liftbottomleft, tmotorVex393HighSpeed_MC29, PIDControl, reversed, encoderPort, I2C_4) #pragma config(Motor, port9, frontRight, tmotorVex393HighSpeed_MC29, PIDControl, reversed, encoderPort, dgtl11) #pragma config(Motor, port10, backRight, tmotorVex393HighSpeed_HBridge, PIDControl, encoderPort, dgtl7) //*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
task main() { // Variables pour roues int X2 = 0, Y1 = 0, X1 = 0, threshold = 15;
// Variables pour ascenseur et gripper
int gripperpotclose = 300; int gripperpotopen = 2500;
while(1 == 1) {
// Code pour roues if(abs(vexRT[Ch3]) > threshold) Y1 = vexRT[Ch3]; else Y1 = 0; if(abs(vexRT[Ch4]) > threshold) X1 = vexRT[Ch4]; else X1 = 0; if(abs(vexRT[Ch1]) > threshold) X2 = vexRT[Ch1]; else X2 = 0;
motor[frontRight] = Y1 - X2 - X1; motor[backRight] = Y1 - X2 + X1; motor[frontLeft] = Y1 + X2 + X1; motor[backLeft] = Y1 + X2 - X1;
resetMotorEncoder(lifttopright); resetMotorEncoder(liftbottomright); resetMotorEncoder(lifttopleft); resetMotorEncoder(liftbottomleft);
// Code pour lancer l'objet bouton 6U if(vexRT[Btn6U] == 1) { motor(lifttopright) = 127; motor(liftbottomright) = 127; motor(lifttopleft) = 127; motor(liftbottomleft) = 127;
if(getMotorEncoder(lifttopright)>200) { if(SensorValue(Gripperpotent) < gripperpotopen) { motor[grippermotor] = -127; } } else motor[grippermotor] = 0; } else if(getMotorEncoder(lifttopright)>5) { motor(lifttopright) = -80; motor(liftbottomright) = -80; motor(lifttopleft) = -80; motor(liftbottomleft) = -80; } else { motor(lifttopright) = 0; motor(liftbottomright) = 0; motor(lifttopleft) = 0; motor(liftbottomleft) = 0; }
// Controle manuel du gripper avec les boutons 8D (ferme) et 7D (ouvre)
if(vexRT(Btn7U)==1) //ferme le gripper { if(SensorValue(Gripperpotent) > gripperpotclose) motor[grippermotor] = 127; } else motor[grippermotor] = 0;
if(vexRT(Btn7D)==1) //ouvre le gripper { if(SensorValue(Gripperpotent) < gripperpotopen) motor[grippermotor] = -127; } else motor[grippermotor] = 0;
// Code pour lever l'ascenseur Bouton 5U if(vexRT[Btn5U] == 1) { motor(lifttopright) = 90; motor(liftbottomright) = 90; motor(lifttopleft) = 90; motor(liftbottomleft) = 90; }
if(vexRT[Btn5D] == 1) { motor(lifttopright) = -75; motor(liftbottomright) = -75; motor(lifttopleft) = -75; motor(liftbottomleft) = -75; }
wait1Msec(20); // ralentir le cycle à 50 Hz (0,02 secondes par cycle) } }
|  |