task main()
{
int tempAngle;
eraseDisplay();
wait1Msec(11);
nxtDisplayStringAt(0, 63, "Minigolf 2013");
nxtDisplayStringAt(0, 48, "GyroSensor:");
nxtDisplayStringAt(0, 40, "SonarSensor:");
//StartTask(GyroDeviceDriver);
StartTask(DistDeviceDriver);
StartTask(getHeading);
while(!_GyrodriverInitFinish) // Gyro initialisierung
{
EndTimeSlice();
}
tempAngle = (int)_fGyroAngle;
wait1Msec(4000);
while(tempAngle != (int)_fGyroAngle)
{
nxtDisplayStringAt(0, 48, "GYRO FAIL, %02d",(int)_fGyroAngle);
nxtDisplayStringAt(0, 40, "Drift after INIT");
}
searchObjekt();
wait1Msec(500);
calc_koordinaten_Ball(_ObjectAngle,_ObjectDistance+45);
wait1Msec(500);
calc_degree_hit();
wait1Msec(500);
calc_koordinaten_Drive();
wait1Msec(500);
turn2Degree(900);
wait1Msec(500);
driveDistance(_distance_X2Drive);
wait1Msec(500);
turn2Degree(0);
wait1Msec(500);
driveDistance(_distance_Y2Drive);
wait1Msec(500);
turn2Degree(_degree_Hit);
wait1Msec(500);
driveDistance(50);
wait1Msec(500);
schlagen(40,60);
while(1)
{
EndTimeSlice();
}
StopAllTasks();
}
task main() {
float pointDistance[] = { 19.0, 10.0, 19.0, 10.0 };
float totalDistance = 0.0;
int i;
startRobot();
waitForStart();
if (PLAY_SOUNDS == true) {
PlaySound(soundBeepBeep);
}
if (WAIT_FOR_START == true) {
wait1Msec(START_DELAY);
}
// Connor's portion
for (i = 0; i < 4; i++) {
if (i == 0 && ALT_ROUTE == true) {
driveDistance(ALT_DISTANCE1, FORWARD, ADJ_NO);
turnRobot(ALT_DEGREES, FORWARD, TURNTYPE_A);
driveDistance(ALT_DISTANCE2, REVERSE, ADJ_NO);
}
else {
driveDistance(pointDistance[i], DIRECTION_A, ADJ_YES);
}
totalDistance += pointDistance[i];
if (QUICK_ROUTE == true || i == 3 || readIR(i+1) == true) {
wait1Msec(250);
servo[blockServo] = 80;
servo[flagServo] = 65;
wait1Msec(500);
servo[blockServo] = 254;
servo[flagServo] = 65;
wait1Msec(250);
break;
}
}
// Devan's portion
if (DIRECTION_A == DIRECTION_B) {
driveDistance(CULM_DISTANCE - totalDistance, DIRECTION_B, ADJ_YES);
}
else {
driveDistance(totalDistance - 6.5, DIRECTION_B, ADJ_NO);
}
turnRobot(TURN_DEGREES_B, DIRECTION_B, TURNTYPE_B);
driveDistance(35.0, DIRECTION_B, ADJ_NO);
turnRobot(TURN_DEGREES_B, DIRECTION_B, TURNTYPE_B);
driveDistance(PARK_DISTANCE, DIRECTION_B, ADJ_NO);
if (PLAY_SOUNDS == true) {
PlaySound(soundBeepBeep);
wait1Msec(500);
}
}
task main()
{
StartTask(Collision);
//StartTask(getHeading);
//wait1Msec(1000);
driveDistance(36, 75);
//reachHeading(92);
//reachHeading(270);
}
//all functions necessary for teleopperiodic
void DriveTrain::Drive(){
float throttle = getThrottle(kThrottleMinimum); //minimum value for throttle in (min, 1)
//if x is pressed let operator pivot
if (xBox->GetRawButton(ControllerConstants::xBoxButtonMap::kBbutton)){
throttle=.5;
}
if (xBox->GetRawButton(ControllerConstants::xBoxButtonMap::kXbutton)){
myRobot->Drive(0.5 * xBox->GetRawAxis(ControllerConstants::xBoxAxisMap::kLSXAxis),kTurnRightFullDegrees);
}//otherwise drive with each stick controlling the robot
else if (leftStick->GetRawButton(2)){
driveDistance(50, -.75); //
leftEncoder->Reset();
rightEncoder->Reset();
}
else if (rightStick->GetRawButton(2)){
driveDistance(50, .75);
leftEncoder->Reset();
rightEncoder->Reset();
}
//else if (leftStick->GetRawButton(2)){
// driveDistance(50,.75);
//}
//interesting. why is the left commented out but not the right?
else{
myRobot->TankDrive(leftStick->GetY()*throttle*-1, rightStick->GetY()*throttle*-1, true);
step=1;
}
//button RT controls power of shooter
if (xBox->GetRawAxis(ControllerConstants::xBoxAxisMap::kRTAxis)>kTriggerThreshold){
shooter->shoot(xBox->GetRawAxis(ControllerConstants::xBoxAxisMap::kRTAxis));
}
//if right bumper is pressed, shoot boulder at 100% power
else if (xBox->GetRawButton(ControllerConstants::xBoxButtonMap::kRBbutton)){
shooter->shoot(kshoot100);
}
//button LB takes in the ball and resets Servo
else if (xBox->GetRawButton(ControllerConstants::xBoxButtonMap::kLBbutton)){
shooter->takeInBall();
servo->SetAngle(kServoRest);
}
//button Y stops motors and resets Servo
else if (xBox->GetRawButton(ControllerConstants::xBoxButtonMap::kYbutton)){
shooter->stopMotors();
servo->SetAngle(kServoRest);
}
//button RB flicks servo to send ball through motors
else if (xBox->GetRawButton(ControllerConstants::xBoxButtonMap::kAbutton)){
servo->SetAngle(kServoShoot);
}
//camera->cameraTeleop();
//lift boulder at left analog stick speed
shooter->angleBall(xBox->GetRawAxis(ControllerConstants::xBoxAxisMap::kLSYAxis));
SmartDashboard::PutNumber("LD", leftEncoder->GetDistance()); //-432
SmartDashboard::PutNumber("RD", rightEncoder->GetDistance()); //630
SmartDashboard::PutNumber("angleTele", ahrs->GetAngle());
//OneStick: myRobot->ArcadeDrive(stick->GetY()*throttle, stick->GetTwist(),true);
}
/*
* set drive parameters from driveParameterBuffer if new parameters are available,
* otherwise stop the car when specified duration expires for security reasons
*/
static void driveTask(void) {
portTickType duration_ms = 0, directionPercent = 0, currentTime = 0,
lastWakeTime = 0, startTime = 0;
driveParameters par;
int8_t queueReturnValue, speed = 0;
double Ergebnis[3];
gps_reducedData_t *own;
lastWakeTime = os_getTime();
for (;;) {
// calls[6]++;
#ifdef SLAM_SCENARIO
os_frequency(&lastWakeTime, 1);
#else
os_frequency(&lastWakeTime, 60);
#endif
currentTime = os_getTime();
//get new drive commands from queue
queueReturnValue = xQueueReceive(driveParameterBuffer, &par, 10);
if (queueReturnValue == pdTRUE) {
directionPercent = par.direction;
speed = par.speed;
startTime = par.startTime;
duration_ms = par.duration;
}
if (hall != start){
start = !start;
if (os_getTime() - driveTime > 400){
distance++;
#ifdef SLAM_SCENARIO
Drive_SetMotor(0);
os_wait(200);
own = get_ownCoords();
SLAM_Algorithm((int32_t)CoordinatesToMap((int32_t)own->x),(int32_t)CoordinatesToMap((int32_t)own->y),(int32_t)uint16DegreeRelativeToX(own->angle),get_accumulateSteeringAngle()/MAX_STEERING_ANGLE, (int32_t) HALL_RESOLUTION, Ergebnis);
// send_Element(22);
// send_Element((int16_t)Ergebnis[0]);
// send_Element((int16_t)Ergebnis[1]);
// send_Element(-1);
set_ownCoords(MapToCoordinates((int16_t)Ergebnis[0]),MapToCoordinates((int16_t)Ergebnis[1]),MapToCoordinates((int16_t)Ergebnis[2]));
accumulateSteeringAngle(INT_LEAST8_MAX);
#endif
}
}
//set speed and direction if specified duration not expired, else stop the car
if (duration_ms < 600){
if (currentTime - startTime < duration_ms) {
#ifdef DEBUG2
printf("\nDRIVE\nset motor speed to %d and angle to %d\n\r", speed,
directionPercent);
#endif
Drive_SetServo(directionPercent);
Drive_SetMotor(speed);
driveDistance();
} else {
Drive_SetMotor(0); //stop if time period expired;
distance = 0;
realdistance = 0;
}
}
else{
Drive_SetServo(directionPercent);
Drive_SetMotor(speed);
if (speed == 0){
distance = 0;
realdistance = 0;
}
driveDistance();
}
}
}
