Permalink
Browse files

work in progress, fleshing out grasper routines

  • Loading branch information...
1 parent 8b4f998 commit ceaf975846324d298cfd9bb5955114eb3f757fd5 @tcwan committed Apr 16, 2012
Showing with 70 additions and 0 deletions.
  1. +70 −0 nxos/systems/tribot/grasper/grasper.S
@@ -142,6 +142,8 @@ claw_closed: .byte 0
/* Inhibition Values */
#define MOTOR_INHIBITED 0x80
#define TONE_INHIBITED 0x00
+#define TONE_ENABLED_FREQ 2500 /* Hz */
+#define TONE_ENABLED_DUR 500 /* ms */
follow_line_suppression: .byte 0
follow_line_portA: .byte MOTOR_INHIBITED
@@ -234,6 +236,7 @@ plap:
bl nx_sound_freq
pop {pc}
+
touch_detected:
push {lr}
bl plip
@@ -576,6 +579,68 @@ found_unknown:
exit_bbr_line_follower:
pop {pc}
+/* Play Tone Behavior
+ *
+ */
+bbr_play_tone:
+ /* Play Tone Algorithm:
+ * Determine if Touch Sensor is Activated
+ * and Claw is Closed
+ * If True, enable Tone Playback
+ * Else, keep silent.
+ */
+ push {lr}
+ ldr r2, =tone_freq /* Pointer to Tone Variables */
+
+ ldr r1, =touched
+ ldrb r0, [r1] /* retrieve Touched status */
+ cmp r0, #FALSE
+ beq keep_silent
+ ldr r1, =claw_closed
+ ldrb r0, [r1] /* retrieve Claw Status */
+ cmp r0, #FALSE
+ beq keep_silent
+
+enable_tone:
+ ldr r0, =TONE_ENABLED_FREQ
+ str r0, [r2] /* Frequency */
+ mov r0, #TONE_ENABLED_DUR
+ str r0, [r2, #4] /* Duration */
+ b exit_bbr_play_tone
+keep_silent:
+ mov r0, #TONE_INHIBITED
+ str r0, [r2] /* Frequency */
+ str r0, [r2, #4] /* Duration */
+
+exit_bbr_play_tone:
+ pop {pc}
+
+/* Grasper Arbiter
+ *
+ */
+
+grasper_arbiter:
+ push {lr}
+exit_grasper_arbiter:
+ pop {pc}
+
+/* Grasper Actuator
+ *
+ */
+grasper_actuation:
+ push {lr}
+
+ /* Speaker Actuation */
+ ldr r2, =tone_freq
+ ldr r0, [r2] /* Frequency */
+ ldr r1, [r2, #4] /* Duration */
+ cmp r0, #TONE_INHIBITED
+ blne nx_sound_freq /* Play Tone if Not TONE_INHIBITED */
+
+exit_grasper_actuation:
+ pop {pc}
+
+
/* init_robot
* Robot Specific Initialization tasks
* Note: Modifies register contents
@@ -600,6 +665,11 @@ init_robot:
ldr r4, =claw_oldpos
str r0, [r4] /* Initialize Old Position = Current position */
+ ldr r2, =tone_freq /* Pointer to Tone Variables */
+ mov r0, #TONE_INHIBITED
+ str r0, [r2] /* Frequency */
+ str r0, [r2, #4] /* Duration */
+
ldr r0, =title
bl nx_progtitle
pop {pc}

0 comments on commit ceaf975

Please sign in to comment.