Diff: robot.cpp

Revision:

48:519deb1d4dff

Parent:

44:4b933e7409e7

Child:

49:7d172c133dbf

diff -r 9c520ebc38e0 -r 519deb1d4dff robot.cpp
--- a/robot.cpp	Thu Apr 03 02:44:36 2014 +0000
+++ b/robot.cpp	Thu Apr 03 06:17:32 2014 +0000
@@ -7,7 +7,8 @@
 //also sets up the control system constants
 robot::robot() : spi(PTD2, PTD3, PTD1), bigenc(spi,PTD0), gyro(PTE0, PTE1),
            /* right(bigenc,0,1,PTA5), left(bigenc,2,3,PTA4), */motors(spi,PTD0), pingLeft(PTA13,PTD5),
-           pingRight(PTC7,PTA16), BTLink(PTC4,PTC3), cont(motors) {
+           pingRight(PTC7,PTA16), BTLink(PTC4,PTC3), cont(motors), wristServo(PTC9), thumbServo(PTC8),
+           stickServo(PTA5), cameraServo(PTA4) {
     
     //bigenc.setDirections(1,-1,1,1);
     //left.setReversed(1);
@@ -23,6 +24,9 @@
     circleX=circleY=rectX=rectY=rectRot=triX=triY=triRot=0;
     cameraMode=-1;
     flameLocation=0;
+    grabPosition();
+    retractStick();
+    farCamera();
 }
 
 //driveforward, but set up so that 
@@ -556,4 +560,52 @@
     cont.setSlowMode(1);
     switchCameraMode(0);
     return found;
+}
+
+int robot::grabPosition(){
+    wristServo.toPosition(-45);
+    thumbServo.toPosition(60);
+    return 1;
+}
+int robot::grab(){
+    thumbServo.toPosition(-10);
+    wait(0.5);
+    cont.start();
+    cont.resetTicks();
+    cont.rampSpeed(-20,-20);
+    //s.toPosition(0);
+    //wait(.5); 
+    while(cont.avgTicks()>-900){
+        wristServo.toPosition((-45)+((35+45)*(-cont.avgTicks()))/900);
+        wait_ms(20);
+    }
+    cont.rampSpeed(0,0);
+    wristServo.toPosition(130);
+    while(!cont.stopped());
+    cont.stop();
+    return 1;
+}
+int robot::retractCamera(){
+    cameraServo.toPosition(160);
+    return 1;
+}
+int robot::farCamera(){
+    cameraServo.toPosition(90);
+    return 1;
+}
+int robot::nearCamera(){
+    cameraServo.toPosition(45);
+    return 1;
+}
+int robot::downCamera(){
+    cameraServo.toPosition(-15);
+    return 1;
+}
+int robot::extendStick(){
+    stickServo.toPosition(-40);
+    return 1;
+}
+int robot::retractStick(){
+    stickServo.toPosition(250);
+    return 1;
 }
\ No newline at end of file