main.cpp

00001 #include "mbed.h"
00002 #include "Servo.h"
00003 #include "rtos.h"
00004 #include "PinDetect.h"
00005 
00006 Servo shoulder(p26); // pwm
00007 Servo elbow(p25); // pwm
00008 Servo wrist(p24); // pwm
00009 Servo hand(p23); // pwm
00010 
00011 PinDetect button(p8);
00012 bool buttonHit = false;
00013 
00014 float currentShoulderPosition;
00015 float currentElbowPosition;
00016 float currentWristPosition;
00017 
00018 void stroke();
00019 void dunk();
00020 
00021 void button_hit_callback (void) {
00022     buttonHit = !buttonHit;
00023 }
00024 
00025 int main()
00026 {   
00027 
00028     currentShoulderPosition = 0.0f;
00029     currentElbowPosition = 0.0f;
00030     currentWristPosition = 0.0f;
00031     
00032     shoulder = currentShoulderPosition;
00033     elbow = currentElbowPosition;
00034     wrist = currentWristPosition;
00035     
00036     // Use internal pullup for pushbutton
00037     button.mode(PullUp);
00038     // Delay for initial pullup to take effect
00039     wait(.001);
00040     // Setup Interrupt callback function for a pb hit
00041     button.attach_deasserted(&button_hit_callback);
00042     // Start sampling pb input using interrupts
00043     button.setSampleFrequency();
00044 
00045         
00046         
00047 while(1){
00048              
00049                 while(buttonHit){
00050                 dunk();
00051                 stroke();
00052                 wait(.1);
00053                 }
00054 }
00055 
00056 
00057 
00058 
00059 
00060 }
00061 
00062 
00063 void stroke(){
00064     
00065     while(currentShoulderPosition >0.0f){
00066         currentShoulderPosition -= .001f;
00067         shoulder = currentShoulderPosition;
00068         wait(.01);
00069         }
00070     
00071 
00072     
00073     for (float i = 0.0f; i < 0.5f; i+=0.01f) {
00074          currentWristPosition =  i/1.7f;
00075          currentElbowPosition = 0.19f + i/2;
00076 
00077         wrist = currentWristPosition;// increments to .29
00078         elbow = currentElbowPosition;//increments to .47
00079         wait(.05);
00080 
00081 
00082     }
00083     for (float i = 0.0f; i < 0.5f; i+=0.01f) {
00084          currentWristPosition -=  i/1.7;
00085          currentElbowPosition  -= i/2;
00086         
00087         wrist = currentWristPosition;//increments to 0.0f
00088         elbow = currentElbowPosition;//increments to .19f
00089         
00090         wait(.05);
00091     } 
00092 }
00093 
00094 void dunk(){
00095         
00096         while(currentShoulderPosition <0.3f){
00097         currentShoulderPosition += .001f;
00098         shoulder = currentShoulderPosition;
00099         wait(.01);
00100         }
00101         
00102         while(currentElbowPosition <0.3f){
00103         currentElbowPosition += .001f;
00104         elbow = currentElbowPosition;
00105         wait(.01);
00106         }
00107 
00108  
00109    
00110     for (float i = 0.0f; i < 0.5f; i+=0.01f) {
00111         
00112         currentWristPosition = 0.2f - i/1.5;//increments to -.2f
00113 
00114         wrist = currentWristPosition;
00115 
00116 
00117      wait(.05);
00118 
00119     }
00120     for (float i = 0.0f; i < 0.5f; i+=0.01f) {
00121  
00122         currentWristPosition = 0.2f + i/1.5;//increments to .47f
00123         wrist = currentWristPosition;
00124 
00125     wait(.05);
00126 
00127     } 
00128     
00129     stroke();
00130     
00131 }
00132 
00133