/*!
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.
}
