// Open the grabbler: // Status: TESTED void closeGrab(){ SERVO_GRAB.write(NUM_SERVO_GRAB_CLOSE); }
int nose::getAngleTails() { static int lastAngle = NOSE_CENTER; static bool missedLineReverse = false; int angle = 0; // value that will be returned int num = 45; // used for the TURN_TO_LINE CASE int index = -1; // first sensor from the left that sees the line int amountSeen = 0; // amount of sensors that see the line // loop through line sensors looking for line for (int i = 0; i < 8; ++i) { if (lineData[i]) { ++amountSeen; if (index == -1) { index = i; } } } /*** CONTROLS CURRENT STATE ***/ switch (stateMap[stateCounter]) { // follow the right wall /*case WALL_FOLLOW_R: if (!isDropping) { driveSpeed = SPEED; } if (wall_LS > DIST_START_TURN) { // approaching box; start small turn stateCounter++; //if you're at the end of the run near the parking zone } else if (pathCounter == 7 && wall_RS < 100 && wall_R < 100) { //STOPIT(); stateCounter++ } else if (pathCounter == 8 && wall_R < 200 && !isDropping) { // we dropped second to last ring; find inner wall stateCounter++; } break;*/ // follow the left wall case WALL_FOLLOW_L: { digitalWrite(LEDG, HIGH); digitalWrite(A8, LOW); if((millis() - rt2Time) > 325){ driveSpeed = SPEED; } if(stateCounter == 6 && !isDropping){ ringServo.write(150); //move arm forward for second box } if (wall_RS > DIST_START_TURN) { // approaching box; start small turn stateCounter++; } break; } ///Where we think the problem is // find inner wall after delivering 3rd ring (part 1) case MIDDLE_AREA1: // move to next step once we find the line if (lineData[7]) {//_CHANGE used to be 0 currentState = MIDDLE_AREA2; } break; /* find inner wall after delivering 3rd ring (part 2) case MIDDLE_AREA2: // turn right once we find the inner wall if (amountSeen == 0) { currentState = LEFT_TURN2; ++pathCounter; //pathCounter = 4 } break;*/ // line following after reversing in middle section case LINE_FOLLOW: digitalWrite(LEDG, HIGH); ringServo.write(0); rt3Time = millis(); if (amountSeen > 2) { // approaching box; start small turn stateCounter++; } break; case LINE_FOLLOW_UNTIL_CORNER: digitalWrite(LEDG, HIGH); rt2Time = millis(); ringServo.write(75); /*if ( (amountSeen > 2 && index < 3) ) { //CHANGE was no ammountseen == 0 // approaching corner stateCounter++; }*/ if ( amountSeen == 0) { ++stateCounter; missedLineReverse = true; } break; // initial small turn case LEFT_TURN1: digitalWrite(LEDG, LOW); // turn hard once we get close to the wall if (wall_LS > 800) { stateCounter++; } break; // hard turn case LEFT_TURN2: rt2Time = millis(); digitalWrite(LEDR, LOW); if (lineData[6]) { stateCounter++; } break; case RT_TO_WALL: if (wall_RS > 900) { stateCounter++; } break; case LT_TO_LINE: if (lineData[4]) { stateCounter++; } break; case LEFT_TURN2_2: rt2Time = millis(); digitalWrite(LEDR, LOW); if (lineData[6]) { stateCounter++; } break; case STRAIGHT_TIME: digitalWrite(LEDG, LOW); // go straight over line if (amountSeen < 3) { stateCounter++; } break; // initial small turn case RIGHT_TURN1: digitalWrite(LEDG, LOW); // turn hard once we get close to the wall if (/*wall_RS > DIST_HARD_TURN ||*/ wall_LS > 900) { stateCounter++; } break; // hard turn case RIGHT_TURN2: digitalWrite(A8, HIGH); rt2Time = millis(); if (lineData[0]) { stateCounter++; } //lastAngle = angle; break; case RIGHT_TURN3: rt3Time = millis(); if(wall_LS > 900){ stateCounter++; } break; case STOP: STOPIT(); break; case FOLLOW_TILL_LOSE: if(amountSeen == 0){ stateCounter++; } break; case LINE_FOLLOW_UNTIL_XING: //rt2Time = millis(); if (wall_RS > DIST_START_TURN) { // approaching box; start small turn stateCounter++; } break; case FOLLOW_TIME: parkTime = millis(); if((millis() - rt2Time) > 300){ stateCounter++; } break; case FOLLOW_TIME_SHORT: parkTime = millis(); if((millis() - rt3Time) > 450){ stateCounter++; } break; case WALL_UNTIL_LOSE: if(wall_LS < 300 && wall_L < 100){ stateCounter++; } break; case STRAIGHTtoWALL: if((millis() - rt2Time) > 200){ stateCounter++; } break; case LEFT_TURN_MAX: if((millis() - rt2Time) > 1000){ stateCounter++; } //lastAngle = angle; break; case GENTLE_RT: rt2Time = millis(); if(index <= 4 && amountSeen > 0){ //CHANGE was == 4 stateCounter++; } break; case GENTLE_RT2: rt2Time = millis(); if(amountSeen == 0){ stateCounter++; } break; case GENTLE_LT: //This 300 may need changed if(lineData[4]){ stateCounter++; } break; case GENTLE_LT_2: digitalWrite(A8, HIGH);//white led if(wall_L > 600){ stateCounter++; } break; case PRE_PARK: rt2Time = millis(); if((millis() - parkTime) > 800){ stateCounter++; } break; case PARK: if((millis() - parkTime) > 300){ stateCounter++; } break; case BACK_UP: //back up for some time if((millis() - rt3Time) > 1400){ stateCounter++; } break; case TURN_TO_LINE: //driveSpeed = 0; rt2Time = millis(); if(index > 4){ stateCounter++; } break; case BACK_UP_STRAIGHT: //driveSpeed = 0; rt3Time = millis(); if((millis() - rt2Time) > 400 + (missedLineReverse ? 20 : 0) ){ // CHANGE added ternay expression stateCounter++; } break; default: digitalWrite(LEDG, HIGH); digitalWrite(LEDR, HIGH); digitalWrite(A8, HIGH); break; } stopRing(); /*** CONTROLS OUTPUT ***/ switch (stateMap[stateCounter]) { // follow the righ wall /*case WALL_FOLLOW_R: angle = NOSE_CENTER - (WALL_DIST - wall_R)/40; if (pathCounter == 8 && wall_R < 100) { angle = NOSE_CENTER; } break;*/ // follow the left wall case WALL_FOLLOW_L: angle = NOSE_CENTER - (WALL_DIST - wall_L)/35; //THURSDAY break; case WALL_UNTIL_LOSE: angle = NOSE_CENTER + (WALL_DIST - wall_L)/32; break; // transition to middle section after dropping 3rd ring // before seeing the line // line follow in the middle after reversing case FOLLOW_TILL_LOSE: if (index == -1) { //we dont see the line angle = lastAngle; }else { // we see the line angle = index * 2; if (amountSeen == 2) { ++angle; } // set angle based on array angle = NOSE_CENTER - SENSOR_ANGLES[angle]; lastAngle = angle; } break; case FOLLOW_TIME: driveSpeed = SPEED; if (index == -1) { //we dont see the line angle = NOSE_CENTER - 30; }else { // we see the line angle = index * 2; if (amountSeen == 2) { ++angle; } // set angle based on array angle = NOSE_CENTER - SENSOR_ANGLES[angle]; lastAngle = angle; } break; case FOLLOW_TIME_SHORT: ringServo.write(160); angle = NOSE_CENTER - 5; break; case LINE_FOLLOW_UNTIL_XING: if((millis() - rt2Time) > 325){ driveSpeed = SPEED; } if (index == -1) { angle = lastAngle; }else { // we see the line angle = index * 2; if (amountSeen == 2) { ++angle; } if(amountSeen == -1){ digitalWrite(LEDR, HIGH); } else{ digitalWrite(LEDR, LOW); } if(amountSeen == 1){ digitalWrite(LEDG, HIGH); } else{ digitalWrite(LEDG, LOW); } // set angle based on array angle = NOSE_CENTER - SENSOR_ANGLES[angle]; lastAngle = angle; } break; case LINE_FOLLOW: if((millis() - rt2Time) > 425){ driveSpeed = SPEED; } if (index == -1) { angle = lastAngle; }else { // we see the line angle = index * 2; if (amountSeen == 2) { ++angle; } if(amountSeen == -1){ digitalWrite(LEDR, HIGH); } else{ digitalWrite(LEDR, LOW); } if(amountSeen == 1){ digitalWrite(LEDG, HIGH); } else{ digitalWrite(LEDG, LOW); } // set angle based on array angle = NOSE_CENTER - SENSOR_ANGLES[angle]; lastAngle = angle; } break; case LINE_FOLLOW_UNTIL_CORNER: if((millis() - rt2Time) > 200){ driveSpeed = TURN_SPEED; } if (index == -1) { angle = lastAngle; }else { // we see the line angle = index * 2; if (amountSeen == 2) { ++angle; } if(amountSeen == -1){ digitalWrite(LEDR, HIGH); } else{ digitalWrite(LEDR, LOW); } if(amountSeen == 1){ digitalWrite(LEDG, HIGH); } else{ digitalWrite(LEDG, LOW); } // set angle based on array angle = NOSE_CENTER - SENSOR_ANGLES[angle]; lastAngle = angle; } break; case RT_TO_WALL: angle = NOSE_CENTER - 25; break; case LT_TO_LINE: angle = NOSE_CENTER + 10; break; // initial small left turn case LEFT_TURN1: angle = NOSE_CENTER; driveSpeed = TURN_SPEED; break; // hard left turn case LEFT_TURN2: driveSpeed = TURN_SPEED; angle = NOSE_CENTER + 35; break; case LEFT_TURN2_2: driveSpeed = TURN_SPEED; angle = NOSE_CENTER + 35; break; case STRAIGHT_TIME: angle = NOSE_CENTER; driveSpeed = SPEED; break; case STRAIGHTtoWALL: angle = NOSE_CENTER - 30; break; case LEFT_TURN_MAX: angle = NOSE_CENTER + 45; //lastAngle = angle; break; // initial small right turn case RIGHT_TURN1: //driveSpeed = SPEED; angle = NOSE_CENTER - 25; break; // hard right turn case RIGHT_TURN2: driveSpeed = TURN_SPEED; angle = 0; break; case RIGHT_TURN3: driveSpeed = TURN_SPEED; angle = NOSE_CENTER - 35; break; case STOP: angle = NOSE_CENTER; break; case GENTLE_RT: angle = NOSE_CENTER - 30; driveSpeed = TURN_SPEED + 40; break; case GENTLE_RT2 : driveSpeed = SPEED; angle = 0; break; case GENTLE_LT: driveSpeed = TURN_SPEED; angle = NOSE_CENTER + 45; break; case GENTLE_LT_2: angle = NOSE_CENTER + 15; break; case PRE_PARK: driveSpeed = 0; ringServo.write(170); break; case PARK: driveSpeed = 0; ringServo.write(170); break; case BACK_UP: angle = NOSE_CENTER + 35; driveSpeed = -90; break; case TURN_TO_LINE: angle = NOSE_CENTER - 45; driveSpeed = 0; break; case BACK_UP_STRAIGHT: //driveSpeed = 0; angle = NOSE_CENTER; driveSpeed = -90; break; } stopRing(); return angle; }
void MotorExecution::run() { if(this->currentSeq == NULL) { cout << "Sequence is not set. \n"; return; } // extract kss into poses, then run command of pos->positionMove(); KasparPose kp; ServoPosition spos; Servo s; double pos; double speed; double *poss = new double[servos->getNumServos()]; double tempTorq = 1; for (int i = 0; i < this->currentSeq->size(); i++) { this->currentSeq->get(i, (this->currentSeqStep)); kp = currentSeqStep.getPose(); int delay = (this->currentSeqStep.getDelay()); int numPos = kp.getNumOfServoPositions(); ACE_OS::printf("Selected pose name is: %s (%d)\n", this->currentSeqStep.getName().c_str(), i); for(int i = 0; i < servos->getNumServos(); i++) { poss[i] = -1; // invalid value that Yarp driver will not respond to -1. } for(int j = 0; j < numPos; j++) { kp.getServoPositionAt(j, spos); spos.getServo(s); if(s.getType() == Servo::AX12) { this->torq->getRefTorque(s.getYarpId(), &tempTorq); } if(tempTorq != 0) { if( spos.getUnscaledPosition(pos) && spos.getUnscaledSpeed(speed)) { // TODO: Do something with YARP to control motor here ACE_OS::printf("Move now.%s %d %d %f %f %d\n", s.getName().c_str(), s.getYarpId(), s.getHardwareId(), pos, speed, j); this->pos->setRefSpeed(s.getYarpId(), speed); poss[s.getYarpId()] = pos; } } } this->pos->positionMove(poss); ACE_OS::printf("Delay is %d\n", delay); // delay for this current pose yarp::os::Time::delay(delay/1000.0); } delete [] poss; poss = NULL; yarp::os::Time::delay(0.8); this->currentSeq = NULL; }
// Open the grabbler: // Status: TESTED void openGrab(){ SERVO_GRAB.write(NUM_SERVO_GRAB_OPEN); }
int setPosR(String pstnR) { posR = pstnR.toInt(); rightservo.write(90 - posR); return posR; }
void rotateAngle(const int angle){ servo.write(angle); }
void loop() { // try to get client EthernetClient client = server.available(); String getStr; if (client) { digitalWrite(greLed,HIGH); boolean currentLineIsBlank = true; boolean indice = false; while (client.connected()) { if (client.available()) { // client data available to read char c = client.read(); // read 1 byte (character) from client // filter GET request // GET /index.html HTTP/1.1 if (c == 'G') { c = client.read(); if (c == 'E') { c = client.read(); if (c == 'T') { Serial.print("GET="); c = client.read(); // space while (true) { c = client.read(); if (c == ' ') { break; } //Serial.println(c); getRequest[getIndi] = c; getIndi++; } Serial.print(getRequest); Serial.println("@"); } } } if (c == '\n' && currentLineIsBlank) { // convert to string and clear char array String getStr(getRequest); Clear(); // if check // first page if (getStr == "/" || getStr == "/index.html") { indice = true; } // ajax GET info else if (getStr.startsWith("/Set?")) { indice = true; int getRequestStart = getStr.indexOf('?' ); int getRequestFirst = getStr.indexOf('=' ); int getRequestFinal = getStr.indexOf('/',2); int getRequestDuall = getStr.indexOf('&' ); // 2 GET if (getRequestDuall>0) { // /index.html?X=20&Y=30 int getRequestSecon = getStr.indexOf('=', getRequestFirst+1); String inputFirst_1 = getStr.substring(getRequestStart+1, getRequestFirst); // X String valueFirst_1 = getStr.substring(getRequestFirst+1, getRequestDuall); // 20 String inputFirst_2 = getStr.substring(getRequestDuall+1, getRequestSecon); // Y String valueFirst_2 = getStr.substring(getRequestSecon+1, getRequestFinal); // 30 if (inputFirst_1 == "X") { Update('X',valueFirst_1.toInt()); Update('Y',valueFirst_2.toInt()); } // reverse else if (inputFirst_1 == "Y") { Update('Y',valueFirst_1.toInt()); Update('X',valueFirst_2.toInt()); } } else { // /index.html?Y=50 or X=50 String inputFirst = getStr.substring(getRequestStart+1, getRequestFirst); // Y String valueFirst = getStr.substring(getRequestFirst+1, getRequestFinal); // 50 if (inputFirst == "X") { Update('X',valueFirst.toInt()); } else if (inputFirst == "Y") { Update('Y',valueFirst.toInt()); } } } //ajax SAVE info else if (getStr.startsWith("/Save/")) { digitalWrite(redLed,HIGH); client.println("HTTP/1.1 200 OK"); client.println("Connection: close"); Serial.println("SAVE"); Serial.println(""); SD.remove(LOG); dataFile = SD.open(LOG, FILE_WRITE); if(dataFile) { // X=123-Y=45- dataFile.print("X="); dataFile.print(X); dataFile.print("-Y="); dataFile.print(Y); dataFile.print("-"); dataFile.close(); } digitalWrite(redLed,LOW); } //ajax RESET info else if (getStr.startsWith("/Reset/")) { digitalWrite(redLed,HIGH); client.println("HTTP/1.1 200 OK"); client.println("Connection: close"); Serial.println("RESET"); Serial.println(""); delay(1); servoX.detach(); servoY.detach(); digitalWrite(redLed,LOW); digitalWrite(resetPin, LOW); } // ajax SET info else if (getStr.startsWith("/Coordinate/")) { digitalWrite(redLed,HIGH); GetValue(); double temp = analogRead(thermRes); int phot = analogRead(photoRes); client.println("HTTP/1.1 200 OK"); client.println("Connection: close"); client.println(); // JSON client.print("{\"coordinate\":{\"X\":"); client.print(X); client.print(",\"Y\":"); client.print(Y); client.print("},\"temp\":"); client.print(GetTemp(temp),1); client.print(",\"light\":"); client.print(phot); client.print(",\"network\":\""); client.print(Ethernet.localIP()); client.print("\",\"file\":["); for (int i=0; i<HTTP_FILE; i++) { File dFile = SD.open(GET[i]); if (dFile) { if(i>0) { client.print(","); } client.print("{"); client.print("\"name\":\""); client.print(GET[i]); client.print("\",\"size\":"); client.print(dFile.size()); client.print("}"); } } client.println("]}"); digitalWrite(redLed,LOW); } // print other file else { for(int i=1; i<HTTP_FILE; i++) { if (getStr == TYP[0][i]) { webFile = SD.open(GET[i]); if (webFile) { client.println("HTTP/1.1 200 OK"); client.println(TYP[1][i]); if(TYP[2][i] == "1") { client.println("Content-Encoding: gzip"); } client.print("Content-Length: "); client.println(webFile.size()); client.println("Cache-Control: max-age=302400, public"); client.println("Connection: close"); client.println(); } break; } } } // endif check // print index.html if (indice) { webFile = SD.open(GET[0]); if (webFile) { client.println("HTTP/1.1 200 OK"); client.println(TYP[1][0]); client.print("Content-Length: "); client.println(webFile.size()); client.println("Connection: close"); client.println(); } } // read file and write into web client if (webFile) { while(webFile.available()) { client.write(webFile.read()); } webFile.close(); } break; } if (c == '\n') { // last character on line of received text. Starting new line with next character read currentLineIsBlank = true; } else if (c != '\r') { // you've gotten a character on the current line currentLineIsBlank = false; } } } //delay(1); // give the web browser time to receive the data client.stop(); // close the connection digitalWrite(greLed,LOW); } }
//Bin lid close void close() { pos = 0; myservo.write(pos); delay(10); }
void loop() { myservo.attach(pin); // attaches the servo on pin to the servo object myservo2.attach(pin2); myservo2.write(0); Log("ServoIndex: %d\n", myservo2.read()); Log("ServoIndex in Microseconds: %d\n", myservo2.readMicroseconds()); delay(delayAmount); myservo2.write(180); Log("ServoIndex: %d\n", myservo2.read()); Log("ServoIndex in Microseconds: %d\n", myservo2.readMicroseconds()); /* Tested to work on 9/26 at 4:41pm */ myservo.write(-90); Log("ServoIndex: %d\n", myservo.read()); Log("ServoIndex in Microseconds: %d\n", myservo.readMicroseconds()); delay(delayAmount); myservo.write(0); Log("ServoIndex: %d\n", myservo.read()); Log("ServoIndex in Microseconds: %d\n", myservo.readMicroseconds()); delay(delayAmount); myservo.write(180); Log("ServoIndex: %d\n", myservo.read()); Log("ServoIndex in Microseconds: %d\n", myservo.readMicroseconds()); delay(delayAmount); myservo.write(200); Log("ServoIndex: %d\n", myservo.read()); Log("ServoIndex in Microseconds: %d\n", myservo.readMicroseconds()); delay(delayAmount); /*Tested to work on 9/26 at 4:44pm */ myservo.writeMicroseconds(544); Log("ServoIndex: %d\n", myservo.read()); Log("ServoIndex in Microseconds: %d\n", myservo.readMicroseconds()); delay(delayAmount); myservo.writeMicroseconds(4000); Log("ServoIndex: %d\n", myservo.read()); Log("ServoIndex in Microseconds: %d\n", myservo.readMicroseconds()); delay(delayAmount); Log("ServoIndex: %d\n", myservo.read()); Log("ServoIndex in Microseconds: %d\n", myservo.readMicroseconds()); if (myservo.attached()) { Log("Servo is attached\n"); Log("Servo is detaching\n"); myservo.detach(); if (!myservo.attached()) { Log("Servo is detached\n"); } } else { Log("Servo is not attached\n"); } //for (pos = 0; pos < 180; pos += 1) // goes from 0 degrees to 180 degrees //{ // in steps of 1 degree // myservo.write(pos); // tell servo to go to position in variable 'pos' // delay(15); // waits 15ms for the servo to reach the position //} //for (pos = 180; pos >= 1; pos -= 1) // goes from 180 degrees to 0 degrees //{ // myservo.write(pos); // tell servo to go to position in variable 'pos' // delay(15); // waits 15ms for the servo to reach the position //} }
void setup() { hor.attach(6); ver.attach(0); Serial.begin(9600); }
/** * Adjusts the servo gimbal based on the color tracked. * Driving the robot or operating an arm based on color input from gimbal-mounted * camera is currently left as an exercise for the teams. */ void Autonomous(void) { DPRINTF(LOG_DEBUG, "Autonomous"); DPRINTF(LOG_DEBUG, "SERVO - looking for COLOR %s ABOVE %s", td2.name, td1.name); // initialize position and destination variables // position settings range from -1 to 1 // setServoPositions is a wrapper that handles the conversion to range for servo horizontalDestination = 0.0; // final destination range -1.0 to +1.0 verticalDestination = 0.0; // initialize pan variables // incremental tasking toward dest (-1.0 to 1.0) float incrementH, incrementV; // pan needs a 1-up number for each call int panIncrement = 0; // current position range -1.0 to +1.0 horizontalPosition = RangeToNormalized(horizontalServo->Get(),1); verticalPosition = RangeToNormalized(verticalServo->Get(),1); // set servos to start at center position setServoPositions(horizontalDestination, verticalDestination); // for controlling loop execution time float loopTime = 0.1; //float loopTime = 0.05; double currentTime = GetTime(); double lastTime = currentTime; // search variables bool foundColor = 0; double savedImageTimestamp = 0.0; bool staleImage = false; while( IsAutonomous() ) { //GetWatchdog().Feed(); // turn watchdog off while debugging // calculate gimbal position based on colors found if ( FindTwoColors(td1, td2, ABOVE, &par1, &par2) ){ //PrintReport(&par2); foundColor = true; // reset pan panIncrement = 0; if (par1.imageTimestamp == savedImageTimestamp) { // This image has been processed already, // so don't do anything for this loop staleImage = true; DPRINTF(LOG_DEBUG, "STALE IMAGE"); } else { // The target was recognized // save the timestamp staleImage = false; savedImageTimestamp = par1.imageTimestamp; DPRINTF(LOG_DEBUG,"image timetamp: %lf", savedImageTimestamp); // Here is where your game-specific code goes // when you recognize the target // get center of target // Average the color two particles to get center x & y of combined target horizontalDestination = (par1.center_mass_x_normalized + par2.center_mass_x_normalized) / 2; verticalDestination = (par1.center_mass_y_normalized + par2.center_mass_y_normalized) / 2; } } else { // need to pan foundColor = false; } if(foundColor && !staleImage) { /* Move the servo a bit each loop toward the destination. * Alternative ways to task servos are to move immediately vs. * incrementally toward the final destination. Incremental method * reduces the need for calibration of the servo movement while * moving toward the target. */ incrementH = horizontalDestination - horizontalPosition; // you may need to reverse this based on your vertical servo installation //incrementV = verticalPosition - verticalDestination; incrementV = verticalDestination - verticalPosition; adjustServoPositions( incrementH, incrementV ); ShowActivity ("** %s & %s found: Servo: x: %f y: %f ** ", td1.name, td2.name, horizontalDestination, verticalDestination); } else { //if (!staleImage) { // new image, but didn't find two colors // adjust sine wave for panning based on last movement direction if(horizontalDestination > 0.0) { sinStart = PI/2.0; } else { sinStart = -PI/2.0; } /* pan to find color after a short wait to settle servos * panning must start directly after panInit or timing will be off */ if (panIncrement == 3) { panInit(8.0); // number of seconds for a pan } else if (panIncrement > 3) { panForTarget(horizontalServo, sinStart); /* Vertical action: In case the vertical servo is pointed off center, * center the vertical after several loops searching */ if (panIncrement == 20) { verticalServo->Set( 0.5 ); } } panIncrement++; ShowActivity ("** %s and %s not found ", td1.name, td2.name); } // end if found color // sleep to keep loop at constant rate // this helps keep pan consistant // elapsed time can vary significantly due to debug printout currentTime = GetTime(); lastTime = currentTime; if ( loopTime > ElapsedTime(lastTime) ) { Wait( loopTime - ElapsedTime(lastTime) ); // seconds } } // end while DPRINTF(LOG_DEBUG, "end Autonomous"); ShowActivity ("Autonomous end "); } // end autonomous
void stateDependentOperations(STATE_TYPE current_state, unsigned long time) { switch(current_state) { //// Wait till the trial is released. Same for all protocols. case WAIT_TO_START_TRIAL: // Wait until we receive permission to continue wait_to_start_trial.run(); break; //// TRIAL_START. Same for all protocols. case TRIAL_START: start_trial.run(); break; case MOVE_SERVO: // Start the servo moving and its timer // Should immediately go to ROTATE_STEPPER1, while th etimer is running. // After rotating, we'll wait for the timer to be completed. // This object is really more of a timer than a state. // OTOH, could argue that MOVE_SERVO and WAIT_FOR_SERVO_MOVE are // the same state, and this distinction is just between the s_setup // and the rest of it. state_wait_for_servo_move.update(linServo); state_wait_for_servo_move.run(time); break; case ROTATE_STEPPER1: srs1.run(); break; case INTER_ROTATION_PAUSE: state_interrotation_pause.run(time); break; case ROTATE_STEPPER2: srs2.run(); break; case WAIT_FOR_SERVO_MOVE: // This state never actually happens, only MOVE_SERVO state_wait_for_servo_move.run(time); break; case RESPONSE_WINDOW: if (FAKE_RESPONDER) { sfrw.update(touched); sfrw.run(time); } else { srw.update(touched); srw.run(time); } break; case REWARD_L: reward_l.run(time); break; case REWARD_R: reward_r.run(time); break; case POST_REWARD_PAUSE: state_post_reward_pause.run(time); break; case ERROR: // turn the light on digitalWrite(__HWCONSTANTS_H_HOUSE_LIGHT, HIGH); state_error_timeout.run(time); break; case INTER_TRIAL_INTERVAL: // turn the light on digitalWrite(__HWCONSTANTS_H_HOUSE_LIGHT, HIGH); // Move servo back linServo.write(param_values[tpidx_SRV_FAR]); // Announce trial_results state_inter_trial_interval.run(time); break; }; };
int main(int argc, char ** argv) { struct sigaction sa; bzero(&sa, sizeof(sa)); sa.sa_handler = handle; if (0 != sigaction(SIGINT, &sa, 0)) { err(EXIT_FAILURE, "sigaction"); } // Before we attempt to read any tasks and skills from the YAML // file, we need to inform the static type registry about custom // additions such as the HelloGoodbyeSkill. Factory::addSkillType<uta_opspace::HelloGoodbyeSkill>("uta_opspace::HelloGoodbyeSkill"); Factory::addSkillType<uta_opspace::TaskPostureSkill>("uta_opspace::TaskPostureSkill"); Factory::addSkillType<uta_opspace::WriteSkill>("uta_opspace::WriteSkill"); Factory::addSkillType<uta_opspace::StaticAccuracyTest>("uta_opspace::StaticAccuracyTest"); Factory::addSkillType<uta_opspace::DynamicAccuracyTest>("uta_opspace::DynamicAccuracyTest"); Factory::addSkillType<uta_opspace::TrajAccuracyTest>("uta_opspace::TrajAccuracyTest"); Factory::addSkillType<uta_opspace::CircleTest>("uta_opspace::CircleTest"); Factory::addSkillType<uta_opspace::GestureSkill>("uta_opspace::GestureSkill"); Factory::addSkillType<uta_opspace::JointTest>("uta_opspace::JointTest"); ros::init(argc, argv, "wbc_m3_ctrl_servo", ros::init_options::NoSigintHandler); parse_options(argc, argv); ros::NodeHandle node("~"); controller.reset(new ControllerNG("wbc_m3_ctrl::servo")); param_cbs.reset(new ParamCallbacks()); Servo servo; try { if (verbose) { warnx("initializing param callbacks"); } registry.reset(factory->createRegistry()); registry->add(controller); param_cbs->init(node, registry, 1, 100); if (verbose) { warnx("starting servo with %lld Hz", servo_rate); } actual_servo_rate = servo_rate; servo.start(servo_rate); } catch (std::runtime_error const & ee) { errx(EXIT_FAILURE, "failed to start servo: %s", ee.what()); } warnx("started servo RT thread"); ros::Time dbg_t0(ros::Time::now()); ros::Time dump_t0(ros::Time::now()); ros::Duration dbg_dt(0.1); ros::Duration dump_dt(0.05); while (ros::ok()) { ros::Time t1(ros::Time::now()); if (verbose) { if (t1 - dbg_t0 > dbg_dt) { dbg_t0 = t1; servo.skill->dbg(cout, "\n\n**************************************************", ""); controller->dbg(cout, "--------------------------------------------------", ""); cout << "--------------------------------------------------\n"; jspace::pretty_print(model->getState().position_, cout, "jpos", " "); jspace::pretty_print(model->getState().velocity_, cout, "jvel", " "); jspace::pretty_print(model->getState().force_, cout, "jforce", " "); jspace::pretty_print(controller->getCommand(), cout, "gamma", " "); Vector gravity; model->getGravity(gravity); jspace::pretty_print(gravity, cout, "gravity", " "); cout << "servo rate: " << actual_servo_rate << "\n"; } } if (t1 - dump_t0 > dump_dt) { dump_t0 = t1; controller->qhlog(*servo.skill, rt_get_cpu_time_ns() / 1000); } ros::spinOnce(); usleep(10000); // 100Hz-ish } warnx("shutting down"); servo.shutdown(); }
/******************************************************************************* * Function Name : rcCarControl * Description : Parses the incoming API commands and sets the motor control pins accordingly * Input : RC Car commands e.g.: rc,FORWARD rc,BACK * Output : Motor signals * Return : 1 on success and -1 on fail *******************************************************************************/ int rcCarControl(String command) { if(command.substring(3,7) == "STOP") { digitalWrite(leftMotorEnable,LOW); digitalWrite(rightMotorEnable,LOW); digitalWrite(leftMotorDir,LOW); digitalWrite(rightMotorDir,HIGH); lastCommand = command; return 1; } if(command.substring(3,7) == "BACK") { digitalWrite(leftMotorDir,LOW); digitalWrite(rightMotorDir,LOW); digitalWrite(leftMotorEnable,HIGH); digitalWrite(rightMotorEnable,HIGH); lastCommand = command; return 1; } if(command.substring(3,10) == "FORWARD") { digitalWrite(leftMotorDir,HIGH); digitalWrite(rightMotorDir,HIGH); digitalWrite(leftMotorEnable,HIGH); digitalWrite(rightMotorEnable,HIGH); lastCommand = command; return 1; } if(command.substring(3,8) == "RIGHT") { digitalWrite(leftMotorDir,HIGH); digitalWrite(rightMotorDir,LOW); digitalWrite(leftMotorEnable,HIGH); digitalWrite(rightMotorEnable,HIGH); delay(220); rcCarControl(lastCommand); return 1; } if(command.substring(3,7) == "LEFT") { digitalWrite(leftMotorDir,LOW); digitalWrite(rightMotorDir,HIGH); digitalWrite(leftMotorEnable,HIGH); digitalWrite(rightMotorEnable,HIGH); delay(200); rcCarControl(lastCommand); return 1; } if(command.substring(3,7) == "GRAB") { pos = 179; myservo.write(pos); delay(15); rcCarControl(lastCommand); return 1; } if(command.substring(3,10) == "RELEASE") { pos = 1; myservo.write(pos); delay(15); rcCarControl(lastCommand); return 1; } // If none of the commands were executed, return false return -1; }
void setup() { //debug Serial.begin(9600); //pin pinMode(resetPin, OUTPUT); pinMode(sdcPin, OUTPUT); //pinMode(ethPin, OUTPUT); pinMode(greLed, OUTPUT); pinMode(redLed, OUTPUT); // initialize SD card //digitalWrite(ethPin, LOW); //digitalWrite(sdcPin, HIGH); Serial.println("Initializing SD card..."); if (!SD.begin(sdcPin)) { Serial.println("ERROR - SD card initialization failed!"); return; } // check for file for (int i=0; i<HTTP_FILE; i++) { if (!SD.exists(GET[i])) { Serial.print("ERROR - Can't find "); Serial.println(GET[i]); return; // can't find index file } } if (!SD.exists(LOG)) { Serial.print("ERROR - Can't find "); Serial.println(LOG); return; // can't find index file } else { dataFile = SD.open(LOG, FILE_READ); if(dataFile) { char temp[3],c,d; while(dataFile.available()) { // X=123-Y=45- d = (char) dataFile.read(); // X or Y dataFile.read(); // = c = (char) dataFile.read(); for(int i=0;c!='-';i++) { temp[i] = c; c = (char) dataFile.read(); } String Coord(temp); if(d=='X') { Update('X',Coord.toInt()); } else if(d=='Y') { Update('Y',Coord.toInt()); } memset(temp, 0, sizeof temp); } dataFile.close(); } } Serial.println("SUCCESS - SD card initialized."); // initialize Ethernet device //digitalWrite(sdcPin, LOW); //digitalWrite(ethPin, HIGH); Ethernet.begin(mac, ip, gateway, subnet); server.begin(); Serial.println("SUCCESS - Ethernet initialized."); // servo Pin servoX.attach(xPin); servoY.attach(yPin); if(servoX.attached() && servoY.attached()) { Serial.println("SUCCESS - Servo initialized."); } else { Serial.println("ERROR - Servo initialization failed!"); return; } digitalWrite(redLed,LOW); digitalWrite(greLed,LOW); Serial.println("START"); }
void lock_stop(void) { lock = 1; motor.writeMicroseconds(motor_null); servo.writeMicroseconds(servo_null); }
void initServo() { myservo.attach(12); // attaches the servo on pin 9 to the servo object myservo2.attach(13); // attaches the servo on pin 9 to the servo object }
int setPosL(String pstnL) { posL = pstnL.toInt(); leftservo.write(90 + posL + 8); return posL; }
void reset() { digitalWrite(DRIVE_PIN1, 0); digitalWrite(DRIVE_PIN2, 0); myservo.write(DEFAULT_STEER); ping(); }
/** * Runs the motors with arcade steering. */ void OperatorControl(void) { myRobot->SetSafetyEnabled(true); while (IsOperatorControl()) { bool setLimit; double cimValue1= -scaleThrottle(-(stick->GetZ())); //Set desired speed from the Throttle, assuming from -1 to 1, also invert the cim, since we want it to rotate coutnerclockwise/clockwise double cimValue2= scaleThrottle(-(stick->GetZ())); //Set desired speed from the Throttle, assuming from -1 to 1 //For shooter /*if (stick.GetRawButton(1) == true) { //back of the robot is moved forward by pushing forward on the joystick myRobot.ArcadeDrive((stick.GetY()), (stick.GetX()), false); // inverted drive control } else { //front of the robot is moved forward by pushing forward on the joystick myRobot.ArcadeDrive(-(stick.GetY()), (stick.GetX()), false); // normal drive control } */ //For manual button speed control, this sets the speed if (stick->GetRawButton(4) == true) { cim1->Set(cimValue1); //use the value from the throttle to set cim speed cim2->Set(cimValue2);//Get speed from throttle, and then scale it setLimit = true; //Open Ball Stopper } else { cim1->Set(0.0); cim2->Set(0.0); setLimit = false; //Close Ball Stopper } //For precisebelt pickup if (stick->GetRawButton(1) == true) { //back of the robot is moved forward by pushing forward on the joystick if (setLimit == true) { JagBelt->ArcadeDrive(0.1, (stick->GetX()), false); // inverted drive control } else { JagBelt->ArcadeDrive((stick->GetY()), (stick->GetX()), false); // inverted drive control } } else { //front of the robot is moved forward by pushing forward on the joystick if (setLimit == true) { JagBelt->ArcadeDrive(-0.1, (stick->GetX()), false); // inverted drive control } else { JagBelt->ArcadeDrive(-(stick->GetY()), (stick->GetX()), false); // inverted drive control } } //For normal belt pickup /*if (stick->GetRawButton(6) == true) { JagBelt->Drive(1.0, 0); //opens gripper } else { JagBelt->Drive(0.0, 0); //closes gripper } */ //For drive if (rightstick->GetRawButton(1) == true) { //back of the robot is moved forward by pushing forward on the joystick if (rightstick->GetRawButton(10) == true) { precisionMode= true; } else { precisionMode= false; } myRobot->ArcadeDrive((rightstick->GetY()), (rightstick->GetX()), precisionMode); // inverted drive control } else { if (rightstick->GetRawButton(10) == true) { precisionMode= true; } else { precisionMode= false; } //front of the robot is moved forward by pushing forward on the joystick myRobot->ArcadeDrive(-(rightstick->GetY()), (rightstick->GetX()), precisionMode); // normal drive control } /*if (stick->GetRawButton(8) == true) { cim1->Set(-0.37); //run cim 1 at 50% speed counterclockwise?? cim2->Set(0.37); // run cim at 50% speed clockwise check = true; //indicate if motors are running } else if (check == true){ // if motors are running a Wait(2.0); belt->Set(1); // run the belt Wait(2.0); // one sec delay belt->Set(0.0); // turn belt off check = false; // put new check } else if (check == false){ //if false // 2 sec delay to wait for the first ball to shoot cim1->Set(0.0); //stop cims cim2->Set(0.0); } else { // Stop everything cim1->Set(0.0); //stop cims cim2->Set(0.0); belt->Set(0.0); } */ //Code for using window motor if (rightstick->GetRawButton(4)) { window->Set(Relay::kOn); window->Set(Relay::kForward); // tell window motor to go forward } else if (rightstick->GetRawButton(5) == true){ window->Set(Relay::kOn); window->Set(Relay::kReverse); //tell window motor to go backward } else { //Wait(1.0); window->Set(Relay::kOff); //turn it off, if the relays aint being used } //Code for Banebot Motor for stopping ballz if (stick->GetRawButton(2) == true) { // press button TWO to close pickStop->Set(-0.5); //closes ball stopper Wait(1); pickStop->Set(0.0); } else if (stick->GetRawButton(3) == true){ //press button three to open pickStop->Set(0.5); //opens ball stopper Wait(1.2); // too slow, so needs more time pickStop->Set(0.0); } else if (stick->GetRawButton(5)== true){ //press 5 to stop imediately, useful for adjusting angles... //Wait(1.0); pickStop->Set(0.0); } //Code for ... servoooo if (stick->GetRawButton(10) == true) { //press 10 on the left stick... bar->SetAngle(60); // set the angles to 60...clockwise? bar2->SetAngle(60); } else if (stick->GetRawButton(11) == true) // press 11 on the left stick { bar->SetAngle(-60); //set the angles to -60...counterclockwise? bar2->SetAngle(-60); } // Initialize functions... // RelayServo(); //PreciseBelt(); //UltrasonicRange(); // Accelerometer(); //dash->GetPacketNumber(); // send data back to dashboard dash->GetPacketNumber(); //not sure why this is here 0_0 //int limitValue= limitSwitch->Get() ? 1 : 0; // retrieve 1 and 0 only.../ look for 0 and 1 float servoAngle = bar->GetAngle(); //dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Limit State: %d", limitValue); //send data back to driver station... // dsLCD->Printf(DriverStationLCD::kUser_Line2, 2, "Servo Angle: %f", servoAngle); //send data back to driver station... float gyroVal = gyro -> GetVoltage();//Gets voltage from gyro float ultraVal = rangeFinder -> GetVoltage(); //Get voltage from ultrasonic sensor float tempVal = Temperature -> GetVoltage();//Gets temperature //Do the math to convert data received from the ultrasonic volts->miliVolts->milivolts per inch->inches float Vm = (ultraVal*1000); float Ri = (Vm/9.765625); // Print data back to dashboard dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Ultrasonic Range: %f",Ri); dsLCD->Printf(DriverStationLCD::kUser_Line2, 1, "Gyro: %f", gyroVal); dsLCD->Printf(DriverStationLCD::kUser_Line3, 1, "Temperature: %f", tempVal); dsLCD->Printf(DriverStationLCD::kUser_Line4, 1, "Cim1 Speed: %f%%", (cimValue1*100)); //display speed that the mototrs are reunning at different percentages... dsLCD->Printf(DriverStationLCD::kUser_Line5, 1, "Cim2 Speed: %f%%", (cimValue2*100)); //Update dashboard dsLCD->UpdateLCD(); } }
void steer() { int angle = readArgument(90); myservo.write(angle); ping(); }
void setServoPin(const int pin){ servo.attach(pin); servo.write(90); }
void setup() { Serial.begin(9600); servo.attach(9); // Attaches the servo on pin 9 to the servo object. servo.write(0); // Resets the position. }