task
task main()
{
resetEncoders();
ClearTimer(T1);
while (true){
if (time1[T1]%5000 == 0)
resetEncoders();
if ((nMotorEncoder[Left]+nMotorEncoder[Right])/2 <= -0.5*in && (nMotorEncoder[Left]+nMotorEncoder[Right])/2 > -2*in){
moveForward(30);
}
if ((nMotorEncoder[Left]+nMotorEncoder[Right])/2 >= 0.5*in && (nMotorEncoder[Left]+nMotorEncoder[Right])/2 < 2*in){
moveForward(-30);
}
if ((nMotorEncoder[Left]+nMotorEncoder[Right])/2 >= 2*in){
moveForward(-100);
}
if ((nMotorEncoder[Left]+nMotorEncoder[Right])/2 <= -2*in){
moveForward(100);
}
if ((nMotorEncoder[Left]+nMotorEncoder[Right])/2 > -0.5*in){
if ((nMotorEncoder[Left]+nMotorEncoder[Right])/2 < 0.5*in){
brake();
}
}
}
}
void forward( int rotations, int power ){
resetEncoders(); //Resets the motor encoder readings.
while( (nMotorEncoder[leftWheel1] < rotations) && (nMotorEncoder[rightWheel1] < rotations) && (nMotorEncoder[leftWheel2]) < rotations && (nMotorEncoder[rightWheel2] < rotations)){
showStatus(rotations, power);
motor[leftWheel1] = power;
motor[rightWheel1] = power;
motor[leftWheel2] = power;
motor[rightWheel2] = power;
}//goes forward until one of the two sides has rotated enough
stopMotors();
if( ( nMotorEncoder[leftWheel1] + nMotorEncoder[leftWheel2] ) - ( nMotorEncoder[rightWheel1] + nMotorEncoder[rightWheel2] ) > 100){
//if shifted to face left
while( nMotorEncoder[leftWheel1] - nMotorEncoder[rightWheel1] > 0 ){
motor[rightWheel1] = power / 5;//turns to be straight
}
stopMotors();
resetEncoders();
}
if( ( nMotorEncoder[rightWheel1] + nMotorEncoder[rightWheel2] ) - ( nMotorEncoder[leftWheel1] + nMotorEncoder[leftWheel2] ) > 100 ){
//if shifted to face right
while( nMotorEncoder[rightWheel1] - nMotorEncoder[leftWheel1] > 0 ){
motor[leftWheel1] = power/5;//turns to be straight
}
}
stopMotors();
resetEncoders();
}//forward function
void Movement::rotateTicks(int ticks, int speed)
{
#ifndef SIMULATION
//Don't do anything if a rotation of zero is inputted
if (ticks != 0)
{
resetEncoders();
//Initialise variables to hold ticks and speed
int leftTicks = 0;
int rightTicks = 0;
int leftSpeed = speed;
int rightSpeed = speed;
//adjust speed and angle if turning anti-clockwise
if (ticks < 0)
{
leftSpeed = (-speed);
rightSpeed = speed;
}
//adjust speed and angle if turning clockwise
else if (ticks > 0)
{
leftSpeed = speed;
rightSpeed = (-speed);
}
while ( (abs(leftTicks) < abs(ticks) ) || (abs(rightTicks) < abs(ticks)) )
{
//adjust motor speed to compensate for error
/*int error = tickError();
if (error > 0)
{
leftSpeed -= 1;
}
else if (error < 0)
{
rightSpeed -= 1;
}*/
//send message to motors to adjust speed
motors->left(leftSpeed);
motors->right(rightSpeed);
delay(1); //Delay 1ms so we don't flood the Serial line
//check ticks to see if we've moved far enough
leftTicks = abs(wheelEnc.getCountsLeft());
rightTicks = abs(wheelEnc.getCountsRight());
}
motors->stop();
resetEncoders();
}
#endif
}
//Rotates the number of ticks specified
//if it doesn't stop turning it means one of the encoders aren't working
//try fiddling with some wires or something...
void Movement::oldRotateTicks(int ticks, int motorSpeed)
{
resetEncoders();
ticks = -ticks; //Old code is reversed
if (ticks < 0)
{
motorSpeed = - motorSpeed;
}
int minSpeed = 28;
int lowSpeed = motorSpeed / 2;
if ( (lowSpeed > 0) && (lowSpeed < minSpeed) )
{
lowSpeed = minSpeed;
}
else if ( (lowSpeed < 0) && (lowSpeed > ( - minSpeed)) )
{
lowSpeed = - minSpeed;
}
int error, motorOne, motorTwo;
do
{
motorOne = abs(wheelEnc.getCountsM1());
motorTwo = abs(wheelEnc.getCountsM2());
error = tickError();
if (error < 0)
{
motors->left( - motorSpeed);
motors->right(lowSpeed);
}
else if (error > 0)
{
motors->right(motorSpeed);
motors->left( - lowSpeed);
}
else
{
motors->right(motorSpeed);
motors->left( - motorSpeed);
}
delay(1);
} while( (motorTwo < abs(ticks)) || (motorOne < abs(ticks)) );
motors->stop();
resetEncoders();
}
task main()
{
initGyro();
// waitForStart();
// wait1Msec(2000);
driveTo(10000); // come off the ramp
turnDegrees(-90); // turn so we are pointed to the ball
resetEncoders();
driveTo(-6500, -90); // drive towards ball
turnDegrees(-50); // angle toward ball so we can push it and end up inside the park zone
resetEncoders();
driveTo(-15000, -43); // drive into zone
resetEncoders();
driveTo(1000);
holdSpinners();
}
void setRobotPhysLimit( int distance, char direction, int height, int roller, int tray, int platform, int timer )
{
ClearTimer( T1 );
resetEncoders();
//artificiallyresetGyro();
while(time1[T1] < timer && !SensorValue[TowerLimitL] && !SensorValue[TowerLimitR] )
{
if( direction == 'S' )
setDrive( distance );
else if( direction == 'T' )
spinDrive( distance );
else
wheelDrive( distance, direction );
//setArm( height );
if( height == 0 )
moveArm( -10, -10 );
else
{
if( SensorValue[ButtonBlockL] == 1 && SensorValue[ButtonBlockR] == 1 )
moveArm( 7, 7 );
else if( SensorValue[ButtonBlockL] == 1 && SensorValue[ButtonBlockR] == 0 )
moveArm( 7, 20 );
else if( SensorValue[ButtonBlockL] == 0 && SensorValue[ButtonBlockR] == 1 )
moveArm( 20, 7 );
else
setArm( height );
}
moveRollers( roller, roller );
movePiston( tray, platform );
}
stopMotors();
}
int moveStraight(int distance, int inchesOrEncoders = true, int speed = defaultMovementSpeed)
{
resetEncoders();
int encoderDistance = 0;
if(inchesOrEncoders){
encoderDistance = inchesToEncoder(distance);
}
else {
encoderDistance = distance;
}
if (distance > 0){
SetMotors(speed,speed);
while (averageEncoders() < encoderDistance){
wait1Msec(3);
}
}
else{
SetMotors(-speed,-speed);
while (averageEncoders() > encoderDistance){
wait1Msec(3);
}
}
SetMotors(0,0);
return averageEncoders();
}
task main()
{
initGyro();
waitForStart();
//wait1Msec(5000);
driveTo(10000);
turnDegrees(90);
resetEncoders();
driveTo(-4000, 90);
turnDegrees(60);
resetEncoders();
driveTo(-17000, 60);
resetEncoders();
driveTo(500);
holdSpinners();
}
// moveTicks moves the number of ticks given.
// A positive ticks number will go forward, a negative ticks number
// will go backwards.
// We do not stop after hitting the number of ticks. Call motorStop().
void Movement::oldMoveTicks(int ticks, int motorSpeed)
{
resetEncoders();
if(ticks < 0)
{
motorSpeed = - motorSpeed;
}
int error, motorOne, motorTwo;
do
{
motorOne = abs(wheelEnc.getCountsM1());
motorTwo = abs(wheelEnc.getCountsM2());
error = tickError();
/*if (error < 0)
{
motors->left(motorSpeed);
motors->right(0);
}
else if (error > 0)
{
motors->left(0);
motors->right(motorSpeed);
}
else
{*/
motors->both(motorSpeed, error);
//}
} while ( (motorTwo < abs(ticks)) && (motorOne < abs(ticks)) );
}
void autoFloor(bool useLift)
{
int irValue = getIRReading();
strafeDist(40, 100, dRight);
displayCenteredTextLine(1, "IR = %i", irValue);
if (irValue == 2)
{
strategyA(useLift);
}
else
{
irValue = getIRReading();
if (irValue == 2)
{
strategyA(useLift);
}
else
{
gyroTurn(10, 5, dRight);
int irValue1 = getIRReading();
gyroTurn(10, 10, dLeft);
int irValue2 = getIRReading();
eraseDisplay();
displayCenteredTextLine(2, "IR1 = %i", irValue1);
displayCenteredTextLine(3, "IR2 = %i", irValue2);
if (irValue1 == 2 || irValue2 == 2)
{
gyroTurn(10, 5, dRight);
strategyA(useLift);
}
else
{
strafeDist(40, 75, dRight);
gyroTurn(50, 30, dLeft);
resetEncoders();
while (irValue != 6)
{
irValue = getIRReading();
strafe(100);
int enc = abs(nMotorEncoder[leftBack]);
displayCenteredTextLine(3, "distance=%i", enc);
wait1Msec(1);
}
stopMotors();
int enc = abs(nMotorEncoder[leftBack]);
displayCenteredTextLine(1, "enc = %i", enc);
if (enc < 1500)
{
strategyB(useLift);
}
else
{
strategyC(useLift);
}
}
}
}
}
task main()
{
waitForStart();
initializeRobot();
playback_flight();
resetEncoders();
}
void stopMotors(){
wait1Msec(100);
motor[rightWheel1] = 0;
motor[rightWheel2] = 0;
motor[leftWheel1] = 0;
motor[leftWheel2] = 0;
resetEncoders();
}//stops drive
void initializeRobot(){
resetEncoders();
brake();
motor[Lift1] = 0;
motor[Lift2] = 0;
motor[Sweep] = 0;
motor[Flag] = 0;
servo[bucket] = 72;
servo[pin] = 0;
}
void pre_auton()
{
// All activities that occur before the competition starts
// Example: clearing encoders, setting servo positions, ...
currentX = 0;
currentY = 0;
currentAngle = 0;
resetEncoders();
}
void turnRight(int rotations, int power){
resetEncoders();//resets encoders
while( (nMotorEncoder[leftWheel2] < rotations) && abs(nMotorEncoder[rightWheel2]) < rotations ){
showStatus(rotations, power);
motor[rightWheel2] = 0;
motor[rightWheel1] = 0;
motor[leftWheel2] = power;
motor[leftWheel1] = power;
}
stopMotors();
}//turns right
void setMD49commands(void) {
//set acceleration if command changed since last set of acceleration
if (i2cdata[2] NOT currentAcceleration) {
setAcceleration(i2cdata[2]);
}
//set new mode only if changed
if (i2cdata[3] NOT currentMode) {
setMode(i2cdata[3]);
}
//set speed continuously, when timeout is enabled
if (statusTimeout == 1) {
setSpeed1(i2cdata[0]);
setSpeed2(i2cdata[1]);
}
//set speed once changed, when timeout is disabled
if (statusTimeout == 0) {
if (recentSpeed1 != i2cdata[0]) {
setSpeed1(i2cdata[0]);
}
if (recentSpeed2 != i2cdata[1]) {
setSpeed2(i2cdata[1]);
}
}
//reset encoders if byte was checked
if (i2cdata[4] == 1) {
resetEncoders();
i2cdata[4] = 0;
}
//set regulator if changed
if (i2cdata[5] == 0) {
if (statusRegulator NOT i2cdata[5]) {
disableRegulator();
}
} else if (i2cdata[5] == 1) {
if (statusRegulator NOT i2cdata[5]) {
enableRegulator();
}
}
// set timeout if changed
if (i2cdata[6] == 0) {
if (statusTimeout NOT i2cdata[6]) {
disableTimeout();
}
} else if (i2cdata[6] == 1) {
if (statusTimeout NOT i2cdata[6]) {
enableTimeout();
}
}
}
void initializeRobot() {
disableDiagnosticsDisplay();
//Choose an auto routine.
selectedAutoRoutine = getRouteChoice();
eraseDisplay();
selectedDelayTime = getDelayChoice();
nMotorEncoder[Arm] = 0;
nMotorEncoder[Joint] = 0;
GyroInit(gyroSensor);
resetEncoders();
}
int HBridgeDriver::commandEmergencyStop(void) {
for(int j=0; j < 10; j++) {
int pindex = motorDrive[j][0];
ppwms[pindex]->init(ppwms[pindex]->pin);
ppwms[pindex]->setPWMPrescale(motorDrive[j][2]);
ppwms[pindex]->setPWMResolution(motorDrive[j][3]);
ppwms[pindex]->pwmOff();
}
fault_flag = 16;
resetSpeeds();
resetEncoders();
return 1;
}
/** Go a relative distance and update the current location
*
* @TODO fix code so that it constantly updates position. Currently it only
* updates the location once the final distance has been travelled. This
* could be problematic if the system is interrupted, because it won't
* update the current position
* @TODO fix glitchiness of motor encoders
* @TODO make the code smarter? maybe we can set some vars or something to make
* the code structure a bit less complex.
*
* @PARAM distance the distance to travel in cm
* */
void Helm::goDistance ( float distance ) {
resetEncoders();
if (distance > 0) {
// full ahead
leftMotor .setSpeed(MOTOR_SPEED);
rightMotor.setSpeed(MOTOR_SPEED);
// glitch compensator
usleep(MOTOR_PAUSE);
// drive until each encoder has reached its distance
while ( (leftMotor.getSpeed() != 0) || (rightMotor.getSpeed() != 0) ) {
if ( ( convertToCm( leftEncoder .getPosition() ) >= distance ) && (leftMotor .getSpeed() != 0) )
leftMotor .setSpeed(0);
if ( ( convertToCm( rightEncoder.getPosition() ) >= distance ) && (rightMotor.getSpeed() != 0) )
rightMotor.setSpeed(0);
}
}
if (distance < 0) {
// full back
leftMotor .setSpeed(-MOTOR_SPEED);
rightMotor.setSpeed(-MOTOR_SPEED);
// glitch compensator
usleep(MOTOR_PAUSE);
// drive until each encoder has reached its distance
while ( (leftMotor.getSpeed() != 0) || (rightMotor.getSpeed() != 0) ) {
if ( ( convertToCm( MAX_VALUE - leftEncoder .getPosition() ) >= abs(distance) ) && (leftMotor .getSpeed() != 0) )
leftMotor .setSpeed(0);
if ( ( convertToCm( MAX_VALUE - rightEncoder.getPosition() ) >= abs(distance) ) && (rightMotor.getSpeed() != 0) )
rightMotor.setSpeed(0);
}
}
// update current location
//updateXY( distance );
// glitch compensator
usleep(MOTOR_PAUSE);
}
int main(void) {
init_uart(); // UART initalisieren und einschalten. Alle n�tigen Schnittstellen-Parameter und -Funktionen werden in rs232.h definiert
init_MD49data(); // set defaults for MD49 data and commands
init_twi_slave(SLAVE_ADRESSE); // Init AVR as Slave with Adresse SLAVE_ADRESSE
sei(); // Interrupts enabled
resetEncoders(); // Reset the encoder values to 0
wdt_enable(WDTO_250MS);
while (1) // mainloop
{
setMD49commands(); // set commands on MD49 corresponding to values stored in i2cdata(0-14)
getMD49data(); // get all data from MD49 and save to corresponding values in i2cdata(15-32)
wdt_reset();
} //end.mainloop
} //end.mainfunction
void initializeRobot()
{
// Place code here to sinitialize servos to starting positions.
// Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.
// Initialize encoders
resetEncoders();
// Spawn heading tracking thread
StartTask(heading);
wait1Msec(1000);
return;
}
/** Rotates a specific relative angle and updates current rotation
*
* @TODO fix code so that it constantly updates rotation. Currently it only
* updates the rotation once it has rotated through the entire angle. This
* could be problematic if the system is interrupted, because it won't
* update the current rotation
* @TODO fix glitchiness of motor encoders
* @TODO make the code smarter? maybe we can set some vars or something to make
* the code structure a bit less complex.
* */
void Helm::rotate( float theta ) {
// reset encoders to get a fresh relative reading
resetEncoders();
// rotation is clockwise
if (theta > 0) {
leftMotor .setSpeed(-MOTOR_SPEED);
rightMotor.setSpeed( MOTOR_SPEED);
usleep(MOTOR_PAUSE);
float distance = degToArcLength( theta );
while ( (leftMotor.getSpeed() != 0) || (rightMotor.getSpeed() != 0) ) {
if ( ( convertToCm( MAX_VALUE - leftEncoder.getPosition() ) >= distance ) && (leftMotor.getSpeed() != 0) )
leftMotor .setSpeed(0);
if ( ( convertToCm( rightEncoder.getPosition() ) >= distance ) && (rightMotor.getSpeed() != 0) )
rightMotor.setSpeed(0);
}
}
// rotation is counterclockwise
if (theta < 0) {
leftMotor .setSpeed( MOTOR_SPEED);
rightMotor.setSpeed(-MOTOR_SPEED);
usleep(MOTOR_PAUSE);
float distance = degToArcLength( theta );
while ( (leftMotor.getSpeed() != 0) || (rightMotor.getSpeed() != 0) ) {
if ( ( convertToCm( leftEncoder.getPosition() ) >= abs(distance) ) && (leftMotor.getSpeed() != 0) )
leftMotor .setSpeed(0);
if ( ( convertToCm( MAX_VALUE - rightEncoder.getPosition() ) >= abs(distance) ) && (rightMotor.getSpeed() != 0) )
rightMotor.setSpeed(0);
}
}
// update the current rotation
updateRotation( theta );
// glitch compensator
usleep(MOTOR_PAUSE);
}
RoboClaw::RoboClaw(const char *deviceName, uint16_t address,
uint16_t pulsesPerRev):
_address(address),
_pulsesPerRev(pulsesPerRev),
_uart(0),
_controlPoll(),
_actuators(ORB_ID(actuator_controls_0), 20),
_motor1Position(0),
_motor1Speed(0),
_motor1Overflow(0),
_motor2Position(0),
_motor2Speed(0),
_motor2Overflow(0)
{
// setup control polling
_controlPoll.fd = _actuators.getHandle();
_controlPoll.events = POLLIN;
// start serial port
_uart = open(deviceName, O_RDWR | O_NOCTTY);
if (_uart < 0) { err(1, "could not open %s", deviceName); }
int ret = 0;
struct termios uart_config;
ret = tcgetattr(_uart, &uart_config);
if (ret < 0) { err(1, "failed to get attr"); }
uart_config.c_oflag &= ~ONLCR; // no CR for every LF
ret = cfsetispeed(&uart_config, B38400);
if (ret < 0) { err(1, "failed to set input speed"); }
ret = cfsetospeed(&uart_config, B38400);
if (ret < 0) { err(1, "failed to set output speed"); }
ret = tcsetattr(_uart, TCSANOW, &uart_config);
if (ret < 0) { err(1, "failed to set attr"); }
// setup default settings, reset encoders
resetEncoders();
}
void turn(int direction) {
int mul = direction;
resetEncoders();
//float degreesToTurn = degrees - (degrees/27);
motor[motorD] = 15 * mul;
motor[motorE] = -15 * mul;
float current_rot = HTGYROreadRot(HTGyro) +14;
float time = time1[T1];
float distance = ((prev_rot+current_rot)/2)*time/1000;
total_distance += distance;
prev_rot = current_rot;
clearTimer(T1);
wait1Msec(10);
motor[motorD] = 0;
motor[motorE] = 0;
}
void turn90(int direction){
int turnRadius = (18 * 3.1415926525); //assuming 18 is the width of the robot
if (direction == 1){
int enc = nMotorEncoder[leftFront];
setMovementPower(0,100);
while(abs(nMotorEncoder[leftFront] - enc) < turnRadius){ }
motor[leftFront] = 0;
}
if (direction == -1){
int enc = nMotorEncoder[rightFront];
motor[rightFront] = 100;
//motor[rightOmni] = 100;
while(abs(nMotorEncoder[rightFront] - enc) < turnRadius){}
motor[rightFront] = 0;
//motor[rightOmni] = 0;
}
resetEncoders();
return;
}
task main()
{
initializeRobot();
waitForStart();
while(nMotorEncoder[RightDrive] < 4*360*0.4)
{
moveForward(70);
}
// STEP 2: Deploy auto-scoring arm
servoTarget[autoServo] = 200;
wait1Msec(500);
servoTarget[autoServo] = 255;
wait1Msec(500);
while(nMotorEncoder[RightDrive] < 4*360*0.8)
{
moveForward(70);
}
motor[LeftDrive] = 70;
motor[RightDrive] = -70;
while(true)
{
nxtDisplayCenteredTextLine(3, "Heading: %d", currHeading);
wait1Msec(10);
if (currHeading >= 90 && currHeading < 110) break;
}
halt();
resetEncoders();
wait1Msec(100);
//STEP 7: Drive onto ramp
while(nMotorEncoder[RightDrive] > -(4*360*3))
{
moveForward(-70);
}
halt();
currHeading = 0.0;
wait1Msec(100);
}
void InitMD25 ( void )
{
if ( ( fd = open ( fileName , O_RDWR ) ) < 0 )
{ // Open port for reading and writing
printf ( "Failed to open i2c port\n" ) ;
exit ( 1 ) ;
}
if ( ioctl ( fd , I2C_SLAVE , address ) < 0 )
{ // Set the port options and set the address of the device we wish to speak to
printf ( "Unable to get bus access to talk to slave\n" ) ;
exit ( 1 ) ;
}
resetEncoders ( ) ;
buffer [ 0 ] = 15 ;
buffer [ 1 ] = 1 ;
if ( ( write ( fd , buffer , 2 ) ) != 2 )
{
printf ( "Error writing to i2c slave\r\n" ) ;
exit ( 1 ) ;
}
buffer [ 0 ] = 16 ;
buffer [ 1 ] = 48 ;
if ( ( write ( fd , buffer , 2 ) ) != 2 )
{
printf ( "Error writing to i2c slave\r\n" ) ;
exit ( 1 ) ;
}
buffer [ 0 ] = 14 ;
buffer [ 1 ] = 10 ;
if ( ( write ( fd , buffer , 2 ) ) != 2 )
{
printf ( "Error writing to i2c slave\r\n" ) ;
exit ( 1 ) ;
}
}
task main()
{
initializeRobot();
while(nNxtButtonPressed != nxtOrange)
{
displayStatus();
}
waitTillNoButton();
eraseDisplay();
resetEncoders();
while(nNxtButtonPressed != nxtOrange)
{
while(nxtGrey != nNxtButtonPressed)
{
displayString(4,"PICK UP AND PRESS GREY BTN");
}
while((nMotorEncoder[rightFront] < 1800)||(nMotorEncoder[rightBack] < 1800)||(nMotorEncoder[leftBack] < 1800)||(nMotorEncoder[leftFront]< 1800))
{
eraseDisplay();
setMotors(70,70,70,70);
}
stopMotors();
displayString(1, "should be 1800");
displayString(3, "rightback encoder: %d", nMotorEncoder[rightFront]);
displayString(2, "rightfront encoder: %d", nMotorEncoder[rightBack]);
displayString(4, "leftfront encoder: %d", nMotorEncoder[leftFront]);
displayString(5, "leftback encoder: %d", nMotorEncoder[leftBack]);
}
waitTillNoButton();
}
void InitExperiment ( void )
{
finished = 0 ;
count = 0 ;
missed = 0 ;
accurate = 0 ;
max = 0 ;
resetEncoders ( ) ;
u1k1 = 0 ;
u2k1 = 0 ;
e1k1 = 0 ;
e1k2 = 0 ;
e2k1 = 0 ;
e2k2 = 0 ;
setpoint1 = 0 ;
setpoint2 = 0 ;
prev1 = 0 ;
prev2 = 0 ;
X = 0 ;
Y = 0 ;
Z = 0 ;
StartTimer ( ) ;
}
void ScorbotConsole::start2()
{
//loadParams();
QWidget* centralWidget = new QWidget(this);
QVBoxLayout *mainVertLayout = new QVBoxLayout(centralWidget);
QHBoxLayout *mainHorizLayout = new QHBoxLayout(centralWidget);
mainVertLayout->addLayout(mainHorizLayout);
//Create joint labels
QGridLayout *controlLayout = new QGridLayout(centralWidget);
controlLayout->setVerticalSpacing(0);
int row = 0;
int column = 0;
//Create the horizontal control names
QLabel* jointLbl = new QLabel("<b>Joint</b>", this);
jointLbl->setAlignment(Qt::AlignHCenter);
QLabel* pwmLbl = new QLabel("<b>PWM<b>", this);
pwmLbl->setAlignment(Qt::AlignHCenter);
QLabel* encoderLbl = new QLabel("<b>Encoder Val<b>", this);
encoderLbl->setAlignment(Qt::AlignHCenter);
QLabel* desiredLbl = new QLabel("<b>Desired Encoder<b>", this);
desiredLbl->setAlignment(Qt::AlignHCenter);
QLabel* durationLbl = new QLabel("<b>Duration<b>", this);
durationLbl->setAlignment(Qt::AlignHCenter);
QLabel* setLbl = new QLabel("<b>Set Position<b>", this);
setLbl->setAlignment(Qt::AlignHCenter);
controlLayout->addWidget(jointLbl, row, column++);
controlLayout->addWidget(pwmLbl, row, column++);
controlLayout->addWidget(encoderLbl, row, column++);
controlLayout->addWidget(desiredLbl, row, column++);
controlLayout->addWidget(durationLbl, row, column++);
controlLayout->addWidget(setLbl, row, column++);
controlLayout->setRowStretch(0, 0);
//Create the vertical axis labels
row=1; column=0;
controlLayout->addWidget(new QLabel("<b>Base<b>", this) , row++, column);
controlLayout->addWidget(new QLabel("<b>Shoulder<b>", this), row++, column);
controlLayout->addWidget(new QLabel("<b>Elbow<b>", this) , row++, column);
controlLayout->addWidget(new QLabel("<b>Wrist1<b>", this) , row++, column);
controlLayout->addWidget(new QLabel("<b>Wrist2<b>", this) , row++, column);
controlLayout->addWidget(new QLabel("<b>Gripper<b>", this) , row++, column);
controlLayout->addWidget(new QLabel("<b>Slider<b>", this) , row++, column);
//Create the pwm labels
row=1; column++;
pwmLbls[ScorbotIce::Base] = new QLabel(centralWidget);
controlLayout->addWidget(pwmLbls[ScorbotIce::Base], row++, column);
pwmLbls[ScorbotIce::Shoulder] = new QLabel(centralWidget);
controlLayout->addWidget(pwmLbls[ScorbotIce::Shoulder], row++, column);
pwmLbls[ScorbotIce::Elbow] = new QLabel(centralWidget);
controlLayout->addWidget(pwmLbls[ScorbotIce::Elbow], row++, column);
pwmLbls[ScorbotIce::Wrist1] = new QLabel(centralWidget);
controlLayout->addWidget(pwmLbls[ScorbotIce::Wrist1], row++, column);
pwmLbls[ScorbotIce::Wrist2] = new QLabel(centralWidget);
controlLayout->addWidget(pwmLbls[ScorbotIce::Wrist2], row++, column);
pwmLbls[ScorbotIce::Gripper] = new QLabel(centralWidget);
controlLayout->addWidget(pwmLbls[ScorbotIce::Gripper], row++, column);
pwmLbls[ScorbotIce::Slider] = new QLabel(centralWidget);
controlLayout->addWidget(pwmLbls[ScorbotIce::Slider], row++, column);
for(QMap<ScorbotIce::JointType, QLabel*>::iterator it=pwmLbls.begin(); it!=pwmLbls.end(); ++it)
{ it.value()->setMaximumWidth(50); it.value()->setMinimumWidth(50); }
//Create the encoder labels
row=1; column++;
encoderLbls[ScorbotIce::Base] = new QLabel(centralWidget);
controlLayout->addWidget(encoderLbls[ScorbotIce::Base], row++, column);
encoderLbls[ScorbotIce::Shoulder] = new QLabel(centralWidget);
controlLayout->addWidget(encoderLbls[ScorbotIce::Shoulder], row++, column);
encoderLbls[ScorbotIce::Elbow] = new QLabel(centralWidget);
controlLayout->addWidget(encoderLbls[ScorbotIce::Elbow], row++, column);
encoderLbls[ScorbotIce::Wrist1] = new QLabel(centralWidget);
controlLayout->addWidget(encoderLbls[ScorbotIce::Wrist1], row++, column);
encoderLbls[ScorbotIce::Wrist2] = new QLabel(centralWidget);
controlLayout->addWidget(encoderLbls[ScorbotIce::Wrist2], row++, column);
encoderLbls[ScorbotIce::Gripper] = new QLabel(centralWidget);
controlLayout->addWidget(encoderLbls[ScorbotIce::Gripper], row++, column);
encoderLbls[ScorbotIce::Slider] = new QLabel(centralWidget);
controlLayout->addWidget(encoderLbls[ScorbotIce::Slider], row++, column);
for(QMap<ScorbotIce::JointType, QLabel*>::iterator it=encoderLbls.begin(); it!=encoderLbls.end(); ++it)
{ it.value()->setMaximumWidth(50); it.value()->setMinimumWidth(50); }
//Create the encoder edits
row=1; column++;
encoderEdits[ScorbotIce::Base] = new QLineEdit(centralWidget);
controlLayout->addWidget(encoderEdits[ScorbotIce::Base], row++, column);
encoderEdits[ScorbotIce::Shoulder] = new QLineEdit(centralWidget);
controlLayout->addWidget(encoderEdits[ScorbotIce::Shoulder], row++, column);
encoderEdits[ScorbotIce::Elbow] = new QLineEdit(centralWidget);
controlLayout->addWidget(encoderEdits[ScorbotIce::Elbow], row++, column);
encoderEdits[ScorbotIce::Wrist1] = new QLineEdit(centralWidget);
controlLayout->addWidget(encoderEdits[ScorbotIce::Wrist1], row++, column);
encoderEdits[ScorbotIce::Wrist2] = new QLineEdit(centralWidget);
controlLayout->addWidget(encoderEdits[ScorbotIce::Wrist2], row++, column);
encoderEdits[ScorbotIce::Gripper] = new QLineEdit(centralWidget);
controlLayout->addWidget(encoderEdits[ScorbotIce::Gripper], row++, column);
encoderEdits[ScorbotIce::Slider] = new QLineEdit(centralWidget);
controlLayout->addWidget(encoderEdits[ScorbotIce::Slider], row++, column);
//Create the duration edits
row=1; column++;
durationEdits[ScorbotIce::Base] = new QLineEdit(centralWidget);
controlLayout->addWidget(durationEdits[ScorbotIce::Base], row++, column);
durationEdits[ScorbotIce::Shoulder] = new QLineEdit(centralWidget);
controlLayout->addWidget(durationEdits[ScorbotIce::Shoulder], row++, column);
durationEdits[ScorbotIce::Elbow] = new QLineEdit(centralWidget);
controlLayout->addWidget(durationEdits[ScorbotIce::Elbow], row++, column);
durationEdits[ScorbotIce::Wrist1] = new QLineEdit(centralWidget);
controlLayout->addWidget(durationEdits[ScorbotIce::Wrist1], row++, column);
durationEdits[ScorbotIce::Wrist2] = new QLineEdit(centralWidget);
controlLayout->addWidget(durationEdits[ScorbotIce::Wrist2], row++, column);
durationEdits[ScorbotIce::Gripper] = new QLineEdit(centralWidget);
controlLayout->addWidget(durationEdits[ScorbotIce::Gripper], row++, column);
durationEdits[ScorbotIce::Slider] = new QLineEdit(centralWidget);
controlLayout->addWidget(durationEdits[ScorbotIce::Slider], row++, column);
//Create the set position buttons
row=1; column++;
QSignalMapper* posButtonMapper = new QSignalMapper(this);
setPosButtons[ScorbotIce::Base] = new QPushButton("Set Position", centralWidget);
controlLayout->addWidget(setPosButtons[ScorbotIce::Base], row++, column);
posButtonMapper->setMapping(setPosButtons[ScorbotIce::Base], (int)ScorbotIce::Base);
connect(setPosButtons[ScorbotIce::Base], SIGNAL(pressed()), posButtonMapper, SLOT(map()));
setPosButtons[ScorbotIce::Shoulder] = new QPushButton("Set Position", centralWidget);
controlLayout->addWidget(setPosButtons[ScorbotIce::Shoulder], row++, column);
posButtonMapper->setMapping(setPosButtons[ScorbotIce::Shoulder], (int)ScorbotIce::Shoulder);
connect(setPosButtons[ScorbotIce::Shoulder], SIGNAL(pressed()), posButtonMapper, SLOT(map()));
setPosButtons[ScorbotIce::Elbow] = new QPushButton("Set Position", centralWidget);
controlLayout->addWidget(setPosButtons[ScorbotIce::Elbow], row++, column);
posButtonMapper->setMapping(setPosButtons[ScorbotIce::Elbow], (int)ScorbotIce::Elbow);
connect(setPosButtons[ScorbotIce::Elbow], SIGNAL(pressed()), posButtonMapper, SLOT(map()));
setPosButtons[ScorbotIce::Wrist1] = new QPushButton("Set Position", centralWidget);
controlLayout->addWidget(setPosButtons[ScorbotIce::Wrist1], row++, column);
posButtonMapper->setMapping(setPosButtons[ScorbotIce::Wrist1], (int)ScorbotIce::Wrist1);
connect(setPosButtons[ScorbotIce::Wrist1], SIGNAL(pressed()), posButtonMapper, SLOT(map()));
setPosButtons[ScorbotIce::Wrist2] = new QPushButton("Set Position", centralWidget);
controlLayout->addWidget(setPosButtons[ScorbotIce::Wrist2], row++, column);
posButtonMapper->setMapping(setPosButtons[ScorbotIce::Wrist2], (int)ScorbotIce::Wrist2);
connect(setPosButtons[ScorbotIce::Wrist2], SIGNAL(pressed()), posButtonMapper, SLOT(map()));
setPosButtons[ScorbotIce::Gripper] = new QPushButton("Set Position", centralWidget);
controlLayout->addWidget(setPosButtons[ScorbotIce::Gripper], row++, column);
posButtonMapper->setMapping(setPosButtons[ScorbotIce::Gripper], (int)ScorbotIce::Gripper);
connect(setPosButtons[ScorbotIce::Gripper], SIGNAL(pressed()), posButtonMapper, SLOT(map()));
setPosButtons[ScorbotIce::Slider] = new QPushButton("Set Position", centralWidget);
controlLayout->addWidget(setPosButtons[ScorbotIce::Slider], row++, column);
posButtonMapper->setMapping(setPosButtons[ScorbotIce::Slider], (int)ScorbotIce::Slider);
connect(setPosButtons[ScorbotIce::Slider], SIGNAL(pressed()), posButtonMapper, SLOT(map()));
connect(posButtonMapper, SIGNAL(mapped(int)), this, SLOT(setPosition(int)));
//Gravity Compensation Label
controlLayout->addWidget(new QLabel("<b>Gravity Compensation<b>", this), ++row, 0);
gravityCompLbl = new QLabel(this);
gravityCompLbl->setMaximumWidth(50); gravityCompLbl->setMinimumWidth(50);
controlLayout->addWidget(gravityCompLbl, row++, 1);
//Create the extra buttons
row=1; column=7;
//Reset Encoder Button
QPushButton* resetEncoderButton = new QPushButton("Reset Encoders",centralWidget);
controlLayout->addWidget(resetEncoderButton, row++, column);
connect(resetEncoderButton, SIGNAL(pressed()), this, SLOT(resetEncoders()));
//Enable Motors Button
enableMotorsButton = new QPushButton("Enable Motors", centralWidget);
controlLayout->addWidget(enableMotorsButton, row++, column);
connect(enableMotorsButton, SIGNAL(pressed()), this, SLOT(toggleMotorsEnabled()));
itsMotorsEnabled = false;
itsScorbot->setEnabled(false);
//Print Position Code Buton
QPushButton* printPosCodeButton = new QPushButton("Print Position Code", centralWidget);
controlLayout->addWidget(printPosCodeButton, row++, column);
connect(printPosCodeButton, SIGNAL(pressed()), this, SLOT(printPosCode()));
mainHorizLayout->addLayout(controlLayout);
mainHorizLayout->addSpacing(10);
mainHorizLayout->addStretch(10);
//Make the PID layout
row=1; column = 0;
QGridLayout *pidLayout = new QGridLayout(this);
itsAxisComboBox = new QComboBox(this);
itsAxisComboBox->addItem("Base", 0);
itsAxisComboBox->addItem("Shoulder", 1);
itsAxisComboBox->addItem("Elbow", 2);
itsAxisComboBox->addItem("Wrist1", 3);
itsAxisComboBox->addItem("Wrist2", 4);
itsAxisComboBox->addItem("Gripper", 5);
itsAxisComboBox->addItem("Slider", 6);
connect(itsAxisComboBox, SIGNAL(activated(int)), this, SLOT(pidAxisSelected(int)));
pidLayout->addWidget(itsAxisComboBox, 0, 1);
pGainEdit = new QLineEdit(this);
pidLayout->addWidget(new QLabel("P Gain", this), row, 0);
pidLayout->addWidget(pGainEdit, row++, 1);
iGainEdit = new QLineEdit(this);
pidLayout->addWidget(new QLabel("I Gain", this), row, 0);
pidLayout->addWidget(iGainEdit, row++, 1);
dGainEdit = new QLineEdit(this);
pidLayout->addWidget(new QLabel("D Gain", this), row, 0);
pidLayout->addWidget(dGainEdit, row++, 1);
maxIEdit = new QLineEdit(this);
pidLayout->addWidget(new QLabel("Max I", this), row, 0);
pidLayout->addWidget(maxIEdit, row++, 1);
maxPWMEdit = new QLineEdit(this);
pidLayout->addWidget(new QLabel("Max PWM", this), row, 0);
pidLayout->addWidget(maxPWMEdit, row++, 1);
pwmOffsetEdit = new QLineEdit(this);
pidLayout->addWidget(new QLabel("PWM Offset", this), row, 0);
pidLayout->addWidget(pwmOffsetEdit, row++, 1);
foreArmMassEdit = new QLineEdit(this);
pidLayout->addWidget(new QLabel("Fore Arm Mass", this), row, 0);
pidLayout->addWidget(foreArmMassEdit, row++, 1);
upperArmMassEdit = new QLineEdit(this);
pidLayout->addWidget(new QLabel("Upper Arm Mass", this), row, 0);
pidLayout->addWidget(upperArmMassEdit, row++, 1);
gravityCompScaleEdit = new QLineEdit(this);
pidLayout->addWidget(new QLabel("Gravity Scale", this), row, 0);
pidLayout->addWidget(gravityCompScaleEdit, row++, 1);
QPushButton* setPIDButton = new QPushButton("Set PID", this);
connect(setPIDButton, SIGNAL(pressed()), this, SLOT(setPIDVals()));
pidLayout->addWidget(setPIDButton, row++, 1);
mainHorizLayout->addLayout(pidLayout);
pidAxisSelected(0);
//Create the plot
itsGraphicsScene = new QGraphicsScene(this);
itsGraphicsView = new QGraphicsView(itsGraphicsScene);
itsImageDisplay = new ImageGraphicsItem;
itsGraphicsScene->addItem(itsImageDisplay);
itsGraphicsView->show();
mainVertLayout->addWidget(itsGraphicsView);
itsGraphicsView->setMinimumSize(640, 550);
//Set the main layout to display
setCentralWidget(centralWidget);
centralWidget->setLayout(mainVertLayout);
itsScorbotThread->start();
}
