TETRIX Getting Started Guide FTC Extension. Programming Guide (ROBOTC ) Autonomous Programming

Transcription

1 Introduction In this guide, a TETRIX with LEGO MINDSTORMS robot with an arm and gripper extension will be programmed to move forward until it detects an object, slow down as it approaches the object, and stop to pick up the object when it is within range. The robot will indicate whether the object has been successfully grabbed using the NXT display and by playing a sound. This guide is for use with the ROBOTC programming language. Review: To review the basics of FTC programming in ROBOTC, review the Programming Guides and tutorial videos from the FTC Programming Template extension and the TeleOp Programming extension. These materials contain detailed information about how to use the FCS to run a FTC game and how to use FTC templates to create FCS compatible programs. These materials also contain information about the basics of ROBOTC. Getting Started: 1. Open the autonomous template from the FIRST Tech Challenge folder. Save the file with a new name into an appropriate directory. 2. In the initializerobot() function, set the initial servo positions. Set the gripper servo, servo1, to an initial position of 200, and the arm servo, servo2, to an initial position of 80. Add a one second wait command to allow the servos to move, settle into to their positions, and stop before continuing to the next command. void initializerobot() Place code here to initialize servos to starting positions. Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize. servo[servo1] = 200; servo[servo2] = 80; Initial startup gripper servo position Initial startup arm servo position Wait for arm and gripper to get to position 3. Navigate to the main() function and declare three integer variables outside the While Loop. These variables should be, mvalue, aposition and gposition, representing the sonar value, the arm servo position and the gripper servo position, respectively. These variables will be responsible for keeping a record of the sonar value and the positions of the servos connected to the arm and gripper. int mvalue; int aposition; int gposition; sonar value servo arm position gripper servo position 52

2 TETRIX Getting Started Guide FTC Extension 4. Set the motor speeds to 30 and add a wait statement to allow the motors to be initialized to that speed. motor[motord] = 30; motor[motore] = 30; 5. Inside the While Loop, type the command that stores the value of the Ultrasonic Sensor within the mvalue variable. Also add the commands that will update the NXT screen and display the value of mvalue on line 2 of the NXT Brick display. while (true) mvalue = SensorValue[SonarSensor]; nxtdisplaytextline(2, "Sonar Value: %d", mvalue); nxtdisplaycenteredtextline(4, "Searching"); nxtdisplaycenteredtextline(5, "For Target"); 6. The next step is to write the commands that will allow the robot to decrease its speed when it approaches an obstacle. If the sonar sensor value reads less than 30 cm away from an object, both drive motors should decrease their power to 15. Decrease speed as we approach target if(sensorvalue[sonarsensor] < 30) motor[motord] = 15; motor[motore] = 15; 7. Add a conditional statement that will stop the robot when it is less than 17 centimetres away from the object. Update the display to read Found! Grabbing.. and add a one second wait command as well. Sonar ranging here; Stop at target distance, position gripper and arm if(sensorvalue[sonarsensor] < 17) Target sense distance (centimeters) nxtdisplaycenteredtextline(4, "Found!"); update display nxtdisplaycenteredtextline(5, "Grabbing.."); update display motor[motore] = 0; Motor E is off motor[motord] = 0; Motor D is off Pause for 1 second 53

3 8. Once the robot is in position, add a For Loop that will lower the arm to the same position as the object. Allow 0.5 seconds after the movement has been made before continuing to the next instruction. for (aposition = 100; aposition < 190; aposition++) servo[servo2] = aposition; lower arm Pause for 1/2 second 9. Add two more For Loops to the code. The first For Loop should close the gripper once the arm has been lowered. The next For Loop should raise the arm after the object has been gripped. for (gposition = 180; gposition > 80; gposition--) servo[servo1] = gposition; close gripper Pause for 1/2 second for (aposition = 160; aposition > 25; aposition--) servo[servo2] = aposition; raise arm 10. Add a return statement that will terminate the main function, and therefore end the program, once the object has been gripped. End the Program: 11. Type a closing brace to end the conditional statement in Step 7. 54

4 TETRIX Getting Started Guide FTC Extension Completed Code: #pragma config(hubs, S1, HTMotor, HTServo, none, none) #pragma config(sensor, S2, SonarSensor, sensorsonar) #pragma config(sensor, S4, Touch, sensortouch) #pragma config(motor, mtr_s1_c1_1, motord, tmotornormal, openloop, reversed) #pragma config(motor, mtr_s1_c1_2, motore, tmotornormal, openloop) #pragma config(servo, srvo_s1_c2_1,, tservostandard) #pragma config(servo, srvo_s1_c2_2,, tservostandard) *!!Code automatically generated by 'ROBOTC' configuration wizard!!* / Autonomous Mode Code Template This file contains a template for simplified creation of an autonomous program for a TETRIX robot competition. You need to customize two functions with code unique to your specific robot. / #include "JoystickDriver.c" Include file to "handle" the Bluetooth messages. / initializerobot Prior to the start of autonomous mode, you may want to perform some initialization on your robot. Things that might be performed during initialization include: 1. Move motors and servos to a preset position. 2. Some sensor types take a short while to reach stable values during which time it is best that robot is not moving. For example, gyro sensor needs a few seconds to obtain the background "bias" value. In many cases, you may not have to add any code to this function and it will remain "empty". / void initializerobot() 55

5 Completed Code (continued): Place code here to sinitialize servos to starting positions. Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize. servo[servo1] = 200; servo[servo2] = 80; Initial startup gripper servo position Initial startup arm servo position wait for arm and gripper to get to position / Main Task The following is the main code for the autonomous robot operation. Customize as appropriate for your specific robot. The types of things you might do during the autonomous phase (for the FTC competition) are: 1. Have the robot follow a line on the game field until it reaches one of the puck storage areas. 2. Load pucks into the robot from the storage bin. 3. Stop the robot and wait for autonomous phase to end. This simple template does nothing except play a periodic tone every few seconds. At the end of the autonomous period, the FCS will automatically abort (stop) execution of the program. / task main() initializerobot(); waitforstart(); Wait for the beginning of autonomous phase. 56

6 TETRIX Getting Started Guide FTC Extension Completed Code (continued): / / Add your robot specific autonomous code here. / / int mvalue; int aposition; int gposition; sonar value servo arm position gripper servo position motor[motord] = 30; motor[motore] = 30; while (true) mvalue = SensorValue[SonarSensor]; nxtdisplaytextline(2, "Sonar Value: %d", mvalue); nxtdisplaycenteredtextline(4, "Searching"); nxtdisplaycenteredtextline(5, "For Target"); Decrease speed as we approach target if(sensorvalue[sonarsensor] < 30) motor[motord] = 15; motor[motore] = 15; Sonar ranging here; Stop at target distance, position gripper and arm if(sensorvalue[sonarsensor] < 17) Target sense distance (centimeters) nxtdisplaycenteredtextline(4, "Found!"); update display nxtdisplaycenteredtextline(5, "Grabbing.."); update display motor[motore] = 0; Motor B is off motor[motord] = 0; Motor C is off Pause for 1 second 57

7 Completed Code (continued): for (aposition = 100; aposition < 190; aposition++) servo[servo2] = aposition; lower arm Pause for 1/2 second for (gposition = 180; gposition > 80; gposition--) servo[servo1] = gposition; close gripper Pause for 1/2 second for (aposition = 160; aposition > 25; aposition--) servo[servo2] = aposition; raise arm 58