changeset 269:6774826fd1a5 main

Latest demo code
author Bob Cook <bob@bobcookdev.com>
date Mon, 02 May 2016 19:32:52 -0700
parents 9a53935c6e2d
children c5c1dd29499c
files main/robots/orpheus/test-gripper.cpp
diffstat 1 files changed, 20 insertions(+), 9 deletions(-) [+]
line wrap: on
line diff
--- a/main/robots/orpheus/test-gripper.cpp	Mon May 02 19:32:39 2016 -0700
+++ b/main/robots/orpheus/test-gripper.cpp	Mon May 02 19:32:52 2016 -0700
@@ -238,6 +238,9 @@
 
 // ----------------------------------------------------------------------------------------
 
+static const uint16_t k_gripper_position_open   = 18656;
+static const uint16_t k_gripper_position_closed = 18050;
+
 int main()
 {
     //--    Initialize the hardware.
@@ -246,7 +249,7 @@
 
     //-- Put the servo into an "open" stage.
 
-    OCR1A = 18656;
+    OCR1A = k_gripper_position_open;
     spinwait_delay_ms( 1000 ); // let the servo move
     servo_suspend();
 
@@ -254,7 +257,7 @@
 
     for ( ;; )
     {
-        if ( PIND & ( ( 1 << PD6 ) | ( 1 << PD7 ) ) )
+        if ( ( PIND & ( ( 1 << PD6 ) | ( 1 << PD7 ) ) ) != 0 )
         {
             PORTD |= ( 1 << PD2 );
         }
@@ -267,12 +270,23 @@
         {
             PORTD &= ~( 1 << PD0 );
 
-            OCR1A = 18656;
+            OCR1A = k_gripper_position_open;
             servo_unsuspend();
             spinwait_delay_ms( 1000 ); // let the servo move
 
-            bool canDetection = false;
+            OCR1A = k_gripper_position_closed;
+            spinwait_delay_ms( 2500 ); // let the servo move
 
+            if ( ( PIND & ( ( 1 << PD6 ) | ( 1 << PD7 ) ) ) != 0 )
+            {
+                // can was not detected in the gripper
+                // move the gripper back to the open position
+                OCR1A = k_gripper_position_open;
+                spinwait_delay_ms( 1000 ); // let the servo move
+                servo_suspend();
+            }
+
+            /*
             for ( uint16_t value = 18600; value > 18100; value -= 10 )
             {
                 OCR1A = value;
@@ -287,16 +301,13 @@
 
             if ( ! canDetection )
             {
-                // move the gripper back to the open position
-                OCR1A = 18656;
-                spinwait_delay_ms( 1000 ); // let the servo move
-                servo_suspend();
             }
+            */
         }
         else if ( ( PIND & ( 1 << PD5 ) ) == 0 )
         {
             PORTD &= ~( 1 << PD1 );
-            OCR1A = 18656;
+            OCR1A = k_gripper_position_open;
             spinwait_delay_ms( 1000 ); // let the servo move
             servo_suspend();
         }