Finally, I worked out the final version of controller program and I also made the configuration of the controller. Here it is
#pragma config(Hubs, S1, HTMotor, HTMotor, HTMotor, none)
#pragma config(Hubs, S2, HTServo, none, none, none)
#pragma config(Sensor, S3, IRSensor, sensorHiTechnicIRSeeker1200)
#pragma config(Motor, mtr_S1_C1_1, motorleft, tmotorTetrix, openLoop, reversed, driveLeft, encoder)
#pragma config(Motor, mtr_S1_C1_2, motorright, tmotorTetrix, openLoop, driveRight, encoder)
#pragma config(Motor, mtr_S1_C2_1, arm, tmotorTetrix, openLoop, driveLeft, encoder)
#pragma config(Motor, mtr_S1_C2_2, sweeping, tmotorTetrix, openLoop, reversed)
#pragma config(Motor, mtr_S1_C3_1, upperlift, tmotorTetrix, openLoop)
#pragma config(Motor, mtr_S1_C3_2, lowerlift, tmotorTetrix, openLoop)
#pragma config(Servo, srvo_S2_C1_1, pullarm, tServoStandard)
#pragma config(Servo, srvo_S2_C1_2, cylinder, tServoStandard)
#pragma config(Servo, srvo_S2_C1_3, servo3, tServoNone)
#pragma config(Servo, srvo_S2_C1_4, servo4, tServoNone)
#pragma config(Servo, srvo_S2_C1_5, servo5, tServoNone)
#pragma config(Servo, srvo_S2_C1_6, servo6, tServoNone)
Set up the configuration of ports and controllers.
#include "JoystickDriver.c"
task main()
{
/* readings that keep our robot in perpetual motion. */
while(true) // Infinite loop:
{
getJoystickSettings(joystick);
Joystick x1, y1 reading set relate to the power of motor left and right. The robot control set as tank drive
if(joystick.joy1_y2 > 10 || joystick.joy1_y2 < -10)
{
motor[motorright] = joystick.joy1_y2 / 1.5;
}
if(joystick.joy1_y1 > 10 || joystick.joy1_y1 < -10)
{
motor[motorleft] = joystick.joy1_y1 / 1.5;
}
if((joystick.joy1_y2 < 10) && (joystick.joy1_y2 > -10))
{
motor[motorright] = 0;
}
if((joystick.joy1_y1 < 10) && (joystick.joy1_y1 > -10))
{
motor[motorleft] = 0;
}
if(joy1Btn(8))
{
motor[lowerlift] = 100;
}
else
{
Set up the function of joystick bottoms.
8,6-lower lift up and down
7,5-upper lift up and down
motor[lowerlift] = 0;
}
if(joy1Btn(6))
{
motor[lowerlift] = -30;
}
else
{
motor[lowerlift] = 0;
}
if(joy1Btn(7))
{
motor[upperlift] = 100;
}
else
{
motor[upperlift] = 0;
}
if(joy1Btn(5))
{
motor[upperlift] = -30;
}
else
{
motor[upperlift] = 0;
}
if(joy1Btn(4))
Set up bottom 4 and 2 as move arm to a fix position. Used encoder to record the degree that the motor has turned and let robot stop the motor when the encoder hit certain degree.
{
nMotorEncoder[arm] = 0;
while (nMotorEncoder[arm] > -14)
{
motor[arm] = 15;
}
motor[arm] = 0;
}
if(joy1Btn(2))
{
nMotorEncoder[arm] = 0;
while (nMotorEncoder[arm] < 10)
{
motor[arm] = -15;
}
motor[arm] = 0;
}
if(joy1Btn(3))
{
motor[arm] = 25;
}
else
{
motor[arm] = 0;
}
if(joy1Btn(1))
{
motor[arm] = -10;
}
else
{
motor[arm] = 0;
}
Use bottom 3 and 1 to move the arm up and down freely.
Use top hat bottom to active some other functions.
Cylinder open-0
Pull arm hook-6
if(joystick.joy1_TopHat == 0)//cylinder
{
servo[cylinder] = 225;
}
if(joystick.joy1_TopHat == 6)//pull
{
servo[pullarm] = 80;
}
if(joystick.joy1_TopHat == -1)//return
{
servo[cylinder] = 32;
servo[pullarm] = 225;
motor[sweeping] = 0;
}
if(joystick.joy1_TopHat == 4)//spin
{
motor[sweeping] = 100; // Lower Servo 1 to position 20.
}
if(joystick.joy1_TopHat == 2)//spin
{
motor[sweeping] = -100; // Lower Servo 1 to position 20.
}
}
Use top hat bottom to active some other functions.
Sweep clockwise and counter clock wise – 4 and 2
}
#pragma config(Hubs, S1, HTMotor, HTMotor, HTMotor, none)
#pragma config(Hubs, S2, HTServo, none, none, none)
#pragma config(Sensor, S3, IRSensor, sensorHiTechnicIRSeeker1200)
#pragma config(Motor, mtr_S1_C1_1, motorleft, tmotorTetrix, openLoop, reversed, driveLeft, encoder)
#pragma config(Motor, mtr_S1_C1_2, motorright, tmotorTetrix, openLoop, driveRight, encoder)
#pragma config(Motor, mtr_S1_C2_1, arm, tmotorTetrix, openLoop, driveLeft, encoder)
#pragma config(Motor, mtr_S1_C2_2, sweeping, tmotorTetrix, openLoop, reversed)
#pragma config(Motor, mtr_S1_C3_1, upperlift, tmotorTetrix, openLoop)
#pragma config(Motor, mtr_S1_C3_2, lowerlift, tmotorTetrix, openLoop)
#pragma config(Servo, srvo_S2_C1_1, pullarm, tServoStandard)
#pragma config(Servo, srvo_S2_C1_2, cylinder, tServoStandard)
#pragma config(Servo, srvo_S2_C1_3, servo3, tServoNone)
#pragma config(Servo, srvo_S2_C1_4, servo4, tServoNone)
#pragma config(Servo, srvo_S2_C1_5, servo5, tServoNone)
#pragma config(Servo, srvo_S2_C1_6, servo6, tServoNone)
Set up the configuration of ports and controllers.
#include "JoystickDriver.c"
task main()
{
/* readings that keep our robot in perpetual motion. */
while(true) // Infinite loop:
{
getJoystickSettings(joystick);
Joystick x1, y1 reading set relate to the power of motor left and right. The robot control set as tank drive
if(joystick.joy1_y2 > 10 || joystick.joy1_y2 < -10)
{
motor[motorright] = joystick.joy1_y2 / 1.5;
}
if(joystick.joy1_y1 > 10 || joystick.joy1_y1 < -10)
{
motor[motorleft] = joystick.joy1_y1 / 1.5;
}
if((joystick.joy1_y2 < 10) && (joystick.joy1_y2 > -10))
{
motor[motorright] = 0;
}
if((joystick.joy1_y1 < 10) && (joystick.joy1_y1 > -10))
{
motor[motorleft] = 0;
}
if(joy1Btn(8))
{
motor[lowerlift] = 100;
}
else
{
Set up the function of joystick bottoms.
8,6-lower lift up and down
7,5-upper lift up and down
motor[lowerlift] = 0;
}
if(joy1Btn(6))
{
motor[lowerlift] = -30;
}
else
{
motor[lowerlift] = 0;
}
if(joy1Btn(7))
{
motor[upperlift] = 100;
}
else
{
motor[upperlift] = 0;
}
if(joy1Btn(5))
{
motor[upperlift] = -30;
}
else
{
motor[upperlift] = 0;
}
if(joy1Btn(4))
Set up bottom 4 and 2 as move arm to a fix position. Used encoder to record the degree that the motor has turned and let robot stop the motor when the encoder hit certain degree.
{
nMotorEncoder[arm] = 0;
while (nMotorEncoder[arm] > -14)
{
motor[arm] = 15;
}
motor[arm] = 0;
}
if(joy1Btn(2))
{
nMotorEncoder[arm] = 0;
while (nMotorEncoder[arm] < 10)
{
motor[arm] = -15;
}
motor[arm] = 0;
}
if(joy1Btn(3))
{
motor[arm] = 25;
}
else
{
motor[arm] = 0;
}
if(joy1Btn(1))
{
motor[arm] = -10;
}
else
{
motor[arm] = 0;
}
Use bottom 3 and 1 to move the arm up and down freely.
Use top hat bottom to active some other functions.
Cylinder open-0
Pull arm hook-6
if(joystick.joy1_TopHat == 0)//cylinder
{
servo[cylinder] = 225;
}
if(joystick.joy1_TopHat == 6)//pull
{
servo[pullarm] = 80;
}
if(joystick.joy1_TopHat == -1)//return
{
servo[cylinder] = 32;
servo[pullarm] = 225;
motor[sweeping] = 0;
}
if(joystick.joy1_TopHat == 4)//spin
{
motor[sweeping] = 100; // Lower Servo 1 to position 20.
}
if(joystick.joy1_TopHat == 2)//spin
{
motor[sweeping] = -100; // Lower Servo 1 to position 20.
}
}
Use top hat bottom to active some other functions.
Sweep clockwise and counter clock wise – 4 and 2
}