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