Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #pragma config(Sensor, in2, ArmLF, sensorLineFollower)
- #pragma config(Sensor, in3, CDLF, sensorLineFollower)
- #pragma config(Sensor, dgtl1, distance, sensorSONAR_cm)
- #pragma config(Sensor, dgtl3, eStop, sensorTouch)
- #pragma config(Sensor, dgtl4, en, sensorQuadEncoder)
- #pragma config(Motor, port2, mGrip, tmotorVex393_MC29, openLoop)
- #pragma config(Motor, port3, mSlider, tmotorVex393_MC29, openLoop)
- #pragma config(Motor, port4, mRotate, tmotorServoContinuousRotation, openLoop)
- #pragma config(Motor, port5, mm, tmotorVex393_MC29, openLoop)
- #pragma config(Motor, port6, lidStamper, tmotorVex393_MC29, openLoop)
- #pragma config(Motor, port7, pmCyl, tmotorVex393_MC29, openLoop)
- #pragma config(Motor, port8, chestClose, tmotorVex393_MC29, openLoop)
- #pragma config(Motor, port9, coinStamper, tmotorVex393_MC29, openLoop)
- //*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
- //mm is the cd racks motor
- //cm is the counters motor
- //km is the lid closer motor
- int counter = 0;
- int coinsPassed = 0;
- int cdRotations = 0;
- task e_stop()
- {
- while(true)
- {
- if(SensorValue(eStop) == 1)
- {
- stopAllTasks(); // ends the program and all tasks including task main.
- }
- wait1Msec(10); // prevents the current task from using majority of available CPU capacity
- }
- }
- task cdCounter()
- {
- while(true) {
- if(SensorValue(CDLF) < 1800 && SensorValue(CDLF) > 1400) {
- coinsPassed = coinsPassed + 1;
- waitUntil(SensorValue(CDLF) > 1800); //Wait until coin passes
- }
- if(coinsPassed == 4) {
- coinsPassed = 0;
- }
- wait1Msec(5);
- }
- }
- bool goTo(tMotor m, float point, int speed)
- {
- while(SensorValue(en) != point)
- {
- if(SensorValue(en) > point)
- {
- startMotor(m, -1 * speed);
- }
- else
- {
- startMotor(m, speed);
- }
- }
- stopMotor(m);
- return true;
- }
- /* Will attempt to go to an exact point 6 times
- If after 6 attempts it can't get the exact value requested it will
- accept a value that is +- 1 of the requested value */
- bool goToExactValue(tMotor m, float point)
- {
- int completed = false;
- int fixAttempts = 0; //Here so robot doesn't get stuck and try to correct itself forever
- while(!completed) {
- waitUntil(goTo(m, point, 19));
- wait(.25); //Allow the motor to fully stop before correction
- if(SensorValue(en) != point && fixAttempts <= 5) {
- waitUntil(goTo(m, point, 19));
- fixAttempts = fixAttempts + 1;
- } else {
- completed = (SensorValue(en) == point || SensorValue(en) == point + 1 || SensorValue(en) == point - 1);
- }
- }
- return true;
- }
- bool manipulateGrip(bool open) {
- if(open) {
- startMotor(mGrip, 40);
- } else {
- startMotor(mGrip, -40);
- }
- wait(.5);
- stopMotor(mGrip);
- return true;
- }
- bool moveSlider(float point)
- {
- if(SensorValue(distance) > point) {
- startMotor(mSlider, -25);
- waitUntil(SensorValue(distance) <= point && SensorValue(distance) != -1);
- startMotor(mSlider, 25);
- waitUntil(SensorValue(distance) <= point && SensorValue(distance) != -1);
- } else {
- startMotor(mSlider, 25);
- waitUntil(SensorValue(distance) >= point && SensorValue(distance) != -1);
- startMotor(mSlider, -25);
- waitUntil(SensorValue(distance) >= point && SensorValue(distance) != -1);
- }
- stopMotor(mSlider);
- return true;
- }
- bool rotateArm(bool home) {
- if (home) {
- setServo(mRotate, -15);
- } else {
- setServo(mRotate, -120);
- }
- wait(1);
- return true;
- }
- task cdRack()
- {
- SensorValue(en) = 0;
- //startMotor(cm, 20);
- startTask(cdCounter);
- while(true) {
- if(coinsPassed == 4) {
- //Wiggle before moving
- waitUntil(goTo(mm, (90 * cdRotations) + 10, 100));
- waitUntil(goTo(mm, (90 * cdRotations) - 10, 100));
- cdRotations = cdRotations + 1;
- waitUntil(goToExactValue(mm, 90 * cdRotations));// No longer using the gear makes corrections not needed
- //waitUntil(goTo(mm, 90 * cdRotations, 30));
- }
- }
- }
- task main()
- {
- ///startTask(e_stop);
- startTask(cdRack);
- waitUntil(manipulateGrip(true)); //open gripper
- waitUntil(rotateArm(true)); //Move the arm to the home position
- while(true) {
- if(SensorValue(ArmLF) < 2800 && counter <= 3) {
- waitUntil(manipulateGrip(false)); //close gripper
- int position = (counter * 3) + 6;
- waitUntil(rotateArm(false));//Rotate the arm
- waitUntil(moveSlider(position)); //Move slider to position
- counter = counter + 1;
- waitUntil(manipulateGrip(true)); //open the gripper
- wait(2);
- waitUntil(moveSlider(6)); //Move slider back home
- waitUntil(rotateArm(true)); //Move arm back to home
- if(counter == 3) {
- counter = 0;
- }
- }
- waitInMilliseconds(50);
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement