/*! Change the state of the robot either to stop them, or to set position or speed control. */ vpRobot::vpRobotStateType vpRobotBiclops::setRobotState(vpRobot::vpRobotStateType newState) { switch (newState) { case vpRobot::STATE_STOP: { if (vpRobot::STATE_STOP != getRobotState ()) { stopMotion(); } break; } case vpRobot::STATE_POSITION_CONTROL: { if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState ()) { vpDEBUG_TRACE (12, "Speed to position control."); stopMotion(); } break; } case vpRobot::STATE_VELOCITY_CONTROL: { if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) { vpDEBUG_TRACE (12, "Lock mutex vpEndThread_mutex"); pthread_mutex_lock(&vpEndThread_mutex); vpDEBUG_TRACE (12, "Create speed control thread"); int code; code = pthread_create(&control_thread, NULL, &vpRobotBiclops::vpRobotBiclopsSpeedControlLoop, &controller); if (code != 0) { vpCERROR << "Cannot create speed biclops control thread: " << code << " strErr=" << strerror(errno) << " strCode=" << strerror(code) << std::endl; } controlThreadCreated = true; vpDEBUG_TRACE (12, "Speed control thread created"); } break; } default: break ; } return vpRobot::setRobotState (newState); }
void setMode(void){ if( ~PIND & SW_BUTTON ){ // printf( "PIND is OFF\n"); if( SwitchSts == 0 ){ SwitchSts = 1; } }else{ // printf( "PIND is ON\n"); if( SwitchSts == 1 ){ SwitchSts = 0; mMode++; if( (LED_MAX - mMode) <= 0 ){ PORTC = LED_BAT|LED_TxD|LED_RxD|LED_AUX|LED_MANAGE|LED_PROGRAM; mMode = MODE_0; }else{ PORTC = ~(1 << (LED_MAX - mMode)); } stopMotion(); ServoControl( 0 ); modeCounter = 1; if( mMode == MODE_1 ){ modeWait = mode1act[0][0]; } } } }
void mouse(int button, int state, int x, int y) { if(state == GLUT_DOWN) startMotion(x, y, button, glutGet(GLUT_ELAPSED_TIME)); else if (state == GLUT_UP) stopMotion(x, y, button, glutGet(GLUT_ELAPSED_TIME)); glutPostRedisplay(); }
void mouseButton(int button,int state,int x,int y){ if(button==GLUT_RIGHT_BUTTON) exit(0); if(button==GLUT_LEFT_BUTTON) switch(state){ case GLUT_DOWN: y=winHeight-y;startMotion(x,y);break; case GLUT_UP : stopMotion(x,y);break; } }
void Finger::update(){ if(!finished){ int targetDist = currentPos - targetPos; targetDist = abs(targetDist); int elapsedDist = currentPos - startPos; elapsedDist = abs(elapsedDist); if((millis()-lastTickTime)>TICK_TIMEOUT){ disableMotor(); } else if((direction&¤tPos>=targetPos)|(!direction&¤tPos<=targetPos)){ stopMotion(); }else if(!digitalRead(reed_pin)&&!direction){ Serial.print(name); Serial.println(" hit reed limit."); stopMotion(); } } }
bool RosArmCartesianControl::watchdog() { double watchdog_time = 0.3; if (active==false) { return false; } ros::Time now = ros::Time::now(); ros::Duration time = (now - t_last_command); if ( time > ros::Duration(watchdog_time) ) { active = false; stopMotion(); return false; } return true; }
//right click exits program, left click down begins startMotion, left click up triggers stopMotion void mouse(int button, int state, int x, int y) { //exit program if(button==GLUT_RIGHT_BUTTON) { exit(0); } if(button==GLUT_LEFT_BUTTON) { switch(state) { case GLUT_DOWN: y = winHeight - y; startMotion(x, y); break; case GLUT_UP: stopMotion(x,y); break; } } }
void mouseButton(int button, int state, int x, int y) { switch(button) { case GLUT_LEFT_BUTTON: trackballXform = (GLfloat*)objectXform; break; case GLUT_MIDDLE_BUTTON: trackballXform = (GLfloat*)lightXform; break; } switch(state) { case GLUT_DOWN: startMotion(0, 1, x, y); break; case GLUT_UP: stopMotion(0, 1, x, y); break; } }
// Called when a mouse button is pressed or released void mouseCallback(int button, int state, int x, int y) { switch (button) { case GLUT_LEFT_BUTTON: trackballXform = (float *)objectXform; break; case GLUT_RIGHT_BUTTON: case GLUT_MIDDLE_BUTTON: trackballXform = (float *)lightXform; break; } switch (state) { case GLUT_DOWN: if (button == GLUT_RIGHT_BUTTON) { zoomState = true; lastPos[1] = (float) y; } else if (button == GLUT_MIDDLE_BUTTON) { shiftState = true; lastPos[0] = (float) x; lastPos[1] = (float) y; } else startMotion(0, 1, x, y); break; case GLUT_UP: trackballXform = (float *)lightXform; // turns off mouse effects if (button == GLUT_RIGHT_BUTTON) { zoomState = false; } else if (button == GLUT_MIDDLE_BUTTON) { shiftState = false; } else stopMotion(0, 1, x, y); break; } }
int main(void){ int log = 0; //Start Switch // DDRA = 0x00; // PORTA = 0x12; //Start PORT A for switch and IR sensors DDRA = 0xFC; PORTA = 0xFE; //LED Initial DDRC = 0x7F; PORTC = 0x7E; DDRD = 0x70; PORTD = 0x11; MotorInit(); initSerial(); char * readData = NULL; int isFinish = 0; sensorInit(); if (isCaptureMode ==1) dxl_write_byte( BROADCAST_ID, P_TORQUE_ENABLE, 0 ); while(1){ sensorTest(0); sensorTest(1); sensorTest(2); setMode(); if( checkSerialRead() > 0 ){ readData = getReadBuffer(); if( readData != NULL ){ // printf( "readData=%s\n", &readData[0] ); split( &readData[0] ); switch( serCmd[0] ){ case EVT_ACTION: ServoControl( serCmd[1] ); // setSpeedTest( serCmd[1] ); sendAck(1); break; case EVT_START_MOTION: startMotion( serCmd[1], serCmd[2] ); PORTC = ~(1 << (LED_MAX - 2)); sendAck(1); break; case EVT_STOP_MOTION: stopMotion(); sendAck(1); break; case EVT_FORCE_MOTION: forceMotion( serCmd[1], serCmd[2] ); break; case EVT_GET_NOW_ANGLE: getAngle(); break; case EVT_SET_ANGLE: setAngle(); case EVT_GET_ACT_ANGLE: if( serCmd[1] >= ACT_MAX ){ sendAck(0); }else{ sendActAngle(serCmd[1]); } break; case EVT_GET_LOAD: getLoad(); // printf( "%d\n", movingTime ); break; case EVT_GET_VOLTAGE: getVoltage(); break; case EVT_TORQUE_DISABLE: dxl_write_byte( BROADCAST_ID, P_TORQUE_ENABLE, 0 ); break; case EVT_WATCH_DOG: watchDogCnt = 0; break; case EVT_MOTION_EDIT: break; case 999: // printf( "finish\n"); sendAck(999); isFinish = 1; break; default: sendAck(0); } if( isFinish > 0 ){ MotorControl( 0, 0 ); break; } memset( readData, 0x00, SERIAL_BUFFER_SIZE ); } } memset( &serCmd[0], 0x00, sizeof(int) * SERIAL_BUFFER_SIZE ); if (~PINA & SW_START ) { if (log == 1) printf( "main() 0\n"); if( iStart > 0 ){ iStart = 0; PORTC = LED_BAT|LED_TxD|LED_RxD|LED_AUX|LED_MANAGE|LED_PROGRAM|LED_PLAY; if (isCaptureMode != 1) ServoControl( 0 ); } }else{ if( iStart == 0 ){ PORTC &= ~LED_PLAY; iStart = 1; } if( modeWait <= 0 ){ if (log == 1) printf( "main() 1\n"); setModeAction(); if (isMovetest == 1) { moveTest(); } else { move(); } }else{ if (log == 1) printf( "main() 2\n"); modeWait -= MAIN_DELAY; } } if (sensorValue[0] == 0 && sensorValueOld[0] != sensorValue[0]) { if (log == 1) printf( "### main() sensorValue[0] == 0\n"); PORTC |= LED_PROGRAM; //edit }else if (sensorValueOld[0] != sensorValue[0]){ if (log == 1) printf( "### main() sensorValue[0] == 1\n"); PORTC &= ~LED_PROGRAM; //edit } if (sensorValue[1] == 0 && sensorValueOld[1] != sensorValue[1]) { if (log == 1) printf( "### main() sensorValue[1] == 0\n"); PORTC |= LED_MANAGE; //mon }else if (sensorValueOld[1] != sensorValue[1]){ if (log == 1) printf( "### main() sensorValue[1] == 1\n"); PORTC &= ~LED_MANAGE; //mon } if (sensorValue[2] == 0 && sensorValueOld[2] != sensorValue[2]) { if (log == 1) printf( "### main() sensorValue[2] == 0\n"); PORTC |= LED_AUX; //AUX }else if (sensorValueOld[2] != sensorValue[2]){ if (log == 1) printf( "### main() sensorValue[2] == 1\n"); PORTC &= ~LED_AUX; //AUX } sensorValueOld[0] = sensorValue[0]; sensorValueOld[1] = sensorValue[1]; sensorValueOld[2] = sensorValue[2]; // walk pattern LED // brinkLED(); _delay_ms(MAIN_DELAY); watchDogCnt++; caputureCount1++; if (caputureCount1 == 25){ if (isCaptureMode == 1) getAngle(); caputureCount1 = 0; } } }
task main() { initializeRobot(); if(SensorValue[touch1] == 1){ liftTooLow = true; } while( true ) { //infinite loop for control motorRun = false; liftMotorRun = false; flagMotorRun = false; bucketMoving = false; getJoystickSettings(joystick); /* ===================================================== = Run Controller 1 - Drive Motors ===================================================== */ // Move the robot forwards and backwards if( abs(joystick.joy1_y1) > JOY_THRESHOLD ){ float fPower = (joystick.joy1_y1 * 100 / 128); int iPower = fPower; forward(-iPower); } // Turn the robot left or right if( abs(joystick.joy1_x2) > JOY_THRESHOLD ){ float fPower = (-joystick.joy1_x2 * 100 / 128); int iPower = fPower; turn(iPower); } if( joystick.joy1_TopHat == 4 ){ //moves forward slowly when D-pad is pressed up forward(SLOW_MOVEMENT); } if(joystick.joy1_TopHat == 0 ){ //d-pad down, move back slow forward(-SLOW_MOVEMENT); } if(joystick.joy1_TopHat == 2) { //turns right slowly if the rightmost D-pad is pressed turn(-SLOW_MOVEMENT_TURN); } if(joystick.joy1_TopHat == 6) { //turns left slowly if the leftmost D-pad is pressed turn(SLOW_MOVEMENT_TURN); } /* ===================================================== = Run Controller 2 - Activators ===================================================== */ // Raise and Lower the Lift if(joy2Btn(5) == 1 && liftTooLow == false) { motor[motorLift] = LIFT_POWER; liftMotorRun = true; } if(joy2Btn(7) == 1) { motor[motorLift] = -LIFT_POWER; liftMotorRun = true; } // Run the flag motor forward or backward if(joy2Btn(6) == 1) { motor[motorFlag] = -FLAG_POWER; flagMotorRun = true; } if(joy2Btn(8) == 1) { motor[motorFlag] = (FLAG_POWER); flagMotorRun = true; } if(joy2Btn(2) == 1 && joy2Btn(4) == 1){ servo[bucket] = BEGINNING_POINT + 20; bucketMoving = true; } // Tilt the bucket backward and forward if(joy2Btn(2) == 1 && bucketMoving == false) { servo[bucket] += 1; } if(joy2Btn(4) == 1 && bucketMoving == false) { servo[bucket] -= 1; } if(joy2Btn(1) == 1){ servo[bucket] = BEGINNING_POINT - 40; } // Reset bucket into correct position if(joy2Btn(3) == 1){ servo[bucket] = BEGINNING_POINT + 70; } /* ===================================================== = Stop mottors that are not running ===================================================== */ if( motorRun == false ) { stopMotion(); } if( liftMotorRun == false ) { stopLiftMotor(); } if( flagMotorRun == false ) { stopFlagMotor(); } } }
task main(){ bool liftMotorRun; initializeRobot(); //waitForStart(); getJoystickSettings(joystick); while( true ) { //infinite loop for control motorRun = false; if( abs(joystick.joy1_y1) > threshold ){ //right joystick control. When the left joystick is pushed forward, move forward float fPower = (joystick.joy1_y1 * 100 / 128); int iPower = fPower; forward(iPower); } if( abs(joystick.joy1_x2) > threshold ){ //if right joystick is pulled left or right, turn to go in the same direction. float fPower = (-joystick.joy1_x2 * 100 / 128); int iPower = fPower; turn(iPower); } bool liftTooLow = false; if(SensorValue(touch) == 1){ liftTooLow = true; } if (joy2Btn(1) == 1) { servo[motorLift] = (ServoValue[motorLift] + 1); } if(joy2Btn(3) == 1) { servo[motorLift] = (ServoValue[motorLift] - 1); } if(joy2Btn(2) == 1) { motor[motorLift] = liftPower; liftMotorRun = false; } if(joy2Btn(4) == 1 && liftTooLow == false) { motor[motorLift] = -liftPower; liftMotorRun = false; } if(joy2Btn(2) == 1) { servo[servoBucket] += 1; } if(joy2Btn(5) == 1) { servo[servoFlagAdjust] = 127; } else if(joy2Btn(7) == 1) { servo[servoFlagAdjust] = -127; } else { servo[servoFlagAdjust] = 0; } if(joy2Btn(4) == 1) { servo[servoBucket] -= 1; } if(joy1Btn(6) == 1) { motor[motorFlag] = 50; } if( motorRun == false ){ //stops the motors from running if nothing is pressed. Must be updated to account for all motor functions added to the program. stopMotion(); } if( liftMotorRun == false ){ stopLiftMotion(); } if( abs(joystick.joy2_y1) > threshold ){ motor[motorLift] = (joystick.joy2_y1 * 100) / 128; liftRun = true; } if(liftRun == false){ stopLiftMotion(); } } //if the touch sensor goes off, so does the motor. }