changeset 263:51f23203883c main

Updated
author Bob Cook <bob@bobcookdev.com>
date Sun, 25 Oct 2015 13:44:20 -0700
parents 19ac16c0ea47
children cb0d6aab498d 9a53935c6e2d
files main/robots/orpheus/test-gripper.cpp
diffstat 1 files changed, 102 insertions(+), 2 deletions(-) [+]
line wrap: on
line diff
--- a/main/robots/orpheus/test-gripper.cpp	Tue Oct 13 19:18:48 2015 -0700
+++ b/main/robots/orpheus/test-gripper.cpp	Sun Oct 25 13:44:20 2015 -0700
@@ -76,13 +76,38 @@
 
     DDRB |= ( 1 << PB5 ) | ( 1 << PB6 ); // gripper servo is on PB5
 
-    // center_servos(); // initialize to center positions 
+    OCR1A = 18656; // starting point, determined experimentally
+}
+
+// ----------------------------------------------------------------------------------------
+
+void servo_suspend()
+{
+    // turn off the clock
+    TCCR1B &= ~( ( 1 << CS12  ) | ( 1 << CS11 ) | ( 1 << CS10 ) );
+}
+
+// ----------------------------------------------------------------------------------------
+
+void servo_unsuspend()
+{
+    // re-enable the clock
+    TCCR1B |= ( 0 << CS12  ) | ( 1 << CS11 ) | ( 0 << CS10 ); // clk/8
 }
 
 // ----------------------------------------------------------------------------------------
 
 static void button_init()
 {
+    //-- The gripper detection switches are on PORTD.6 and PORTD.7. Enable the pull-ups.
+
+    DDRD  &= ~( ( 1 << PD6 ) | ( 1 << PD7 ) );
+    PORTD |=    ( 1 << PD6 ) | ( 1 << PD7 );
+
+    //-- The two pushbuttons are on PORTD.4 and PORTD.5. Enable the pull-ups.
+    
+    DDRD  &= ~( ( 1 << PD4 ) | ( 1 << PD5 ) );
+    PORTD |=    ( 1 << PD4 ) | ( 1 << PD5 );
 }
 
 // ----------------------------------------------------------------------------------------
@@ -108,6 +133,16 @@
 
     TIMSK |= ( 1 << OCIE0 );
 
+    //-- The built-in LED is on PORTB.0 and the other three indicator LEDs are on PORTD.0, 
+    //   PORTD.1, and PORTD.2. The internal LED is "on when high" while the other LEDs are
+    //   set to be "on when low".
+
+    DDRB = ( 1 << PB0 );
+    DDRD = ( 1 << PD2 ) | ( 1 << PD1 ) | ( 1 << PD0 );
+
+    PORTB &= ~( 1 << PB0 );
+    PORTD |=  ( 1 << PD2 ) | ( 1 << PD1 ) | ( 1 << PD0 );
+
     //-- Initialize the subsections.
 
     servo_init();
@@ -192,7 +227,13 @@
 
 ISR( TIMER0_COMP_vect )
 {
-    // heartbeat
+    static uint8_t counter = 0;
+
+    if ( ++counter == 5 )
+    {
+        PORTB ^= ( 1 << PB0 );
+        counter = 0;
+    }
 }
 
 // ----------------------------------------------------------------------------------------
@@ -203,10 +244,69 @@
 
     hw_init();
 
+    //-- Put the servo into an "open" stage.
+
+    OCR1A = 18656;
+    spinwait_delay_ms( 1000 ); // let the servo move
+    servo_suspend();
+
     //--    Loop while we have things to do.
 
     for ( ;; )
     {
+        if ( PIND & ( ( 1 << PD6 ) | ( 1 << PD7 ) ) )
+        {
+            PORTD |= ( 1 << PD2 );
+        }
+        else
+        {
+            PORTD &= ~( 1 << PD2 );
+        }
+
+        if ( ( PIND & ( 1 << PD4 ) ) == 0 )
+        {
+            PORTD &= ~( 1 << PD0 );
+
+            OCR1A = 18656;
+            servo_unsuspend();
+            spinwait_delay_ms( 1000 ); // let the servo move
+
+            bool canDetection = false;
+
+            for ( uint16_t value = 18600; value > 18100; value -= 10 )
+            {
+                OCR1A = value;
+                spinwait_delay_ms( 150 );
+
+                if ( ( PIND & ( ( 1 << PD6 ) | ( 1 << PD7 ) ) ) == 0 )
+                {
+                    canDetection = true;
+                    break;
+                }
+            }
+
+            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;
+            spinwait_delay_ms( 1000 ); // let the servo move
+            servo_suspend();
+        }
+        else
+        {
+            PORTD |= ( 1 << PD0 );
+            PORTD |= ( 1 << PD1 );
+        }
+
+        spinwait_delay_ms( 1 );
     }
 
     return 0;