James Hilder
For YRL Robot Arm
Diff: remote.cpp
- Revision:
- 0:b14dfd8816da
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/remote.cpp Fri Mar 03 13:28:54 2017 +0000
@@ -0,0 +1,245 @@
+/* University of York Robotics Laboratory Robot Arm Controller Board
+ *
+ * Robot Arm Remote Control
+ *
+ * File: remote.cpp
+ *
+ * (C) Dept. Electronics & Computer Science, University of York
+ *
+ * James Hilder, Alan Millard, Shuhei Miyashita, Homero Elizondo, Jon Timmis
+ *
+ * February 2017, Version 1.0
+ *
+ */
+
+#include "robotarm.h"
+
+DigitalIn remote_sense(p17,PullUp);
+DigitalIn sw1_up(p21,PullUp);
+DigitalIn sw1_down(p22,PullUp);
+DigitalIn sw1_left(p23,PullUp);
+DigitalIn sw1_right(p24,PullUp);
+DigitalIn sw2_up(p25,PullUp);
+DigitalIn sw2_down(p26,PullUp);
+DigitalIn sw2_left(p29,PullUp);
+DigitalIn sw2_right(p30,PullUp);
+
+DigitalOut red_led(p16);
+DigitalOut green_led(p20);
+Ticker attach_remote_sensor;
+char remote_on = 0;
+char h_switch = 0;
+char limit_reached = 0;
+
+
+void Remote::init()
+{
+ attach_remote_sensor.attach_us(this,&Remote::detect_remote,REMOTE_POLL_RATE);
+}
+
+char step = REMOTE_START_SPEED;
+
+void Remote::detect_direction()
+{
+ char t_switch = 0;
+ if(!sw1_up)t_switch+=1;
+ if(!sw1_down)t_switch+=2;
+ if(!sw1_left)t_switch+=4;
+ if(!sw1_right)t_switch+=8;
+ if(!sw2_up)t_switch+=16;
+ if(!sw2_down)t_switch+=32;
+ if(!sw2_left)t_switch+=64;
+ if(!sw2_right)t_switch+=128;
+
+ if(t_switch!=h_switch) {
+ h_switch=t_switch;
+ pc.printf("Switch ");
+ if(h_switch == 0) {
+ pc.printf("released");
+ step = REMOTE_START_SPEED;
+ set_led(0);
+ } else {
+ if(h_switch & 0x01) {
+ pc.printf("1-UP ");
+ }
+ if(h_switch & 0x02) {
+ pc.printf("1-DN ");
+ }
+ if(h_switch & 0x04) {
+ pc.printf("1-LF ");
+ }
+ if(h_switch & 0x08) {
+ pc.printf("1-RT ");
+ }
+ if(h_switch & 0x10) {
+ pc.printf("2-UP ");
+ }
+ if(h_switch & 0x20) {
+ pc.printf("2-DN ");
+ }
+ if(h_switch & 0x40) {
+ pc.printf("2-LF ");
+ }
+ if(h_switch & 0x80) {
+ pc.printf("2-RT ");
+ }
+
+ }
+ pc.printf("\n");
+ }
+ if(t_switch != 0) {
+
+ if(h_switch & 0x01) {
+ move_servo(1,step);
+ }
+ if(h_switch & 0x02) {
+ move_servo(1,-step);
+ }
+ if(h_switch & 0x04) {
+ move_servo(0,-step);
+ }
+ if(h_switch & 0x08) {
+ move_servo(0,step);
+ }
+ if(h_switch & 0x10) {
+ move_servo(2,step);
+ }
+ if(h_switch & 0x20) {
+ move_servo(2,-step);
+ }
+ if(h_switch & 0x40) {
+ move_servo(3,-step);
+ }
+ if(h_switch & 0x80) {
+ move_servo(3,step);
+ }
+ if(REMOTE_LINEAR_STEPS != 1) {
+ step += REMOTE_START_SPEED;
+ if(step > 200) step = 200;
+ }
+ set_led(2-limit_reached);
+ }
+}
+
+void Remote::move_servo(char servo_number, int adjust)
+{
+ char enabled = 0;
+ char servo_id = 0;
+ short stored_value = 0;
+ switch(servo_number) {
+ case 0:
+ servo_id=BASE;
+ if(HAS_BASE)enabled = 1;
+ stored_value = target_base;
+ break;
+ case 1:
+ servo_id=SHOULDER;
+ if(HAS_SHOULDER) enabled = 1;
+ stored_value = target_shoulder;
+ break;
+ case 2:
+ servo_id=ELBOW;
+ if(HAS_ELBOW) enabled = 1;
+ stored_value = target_elbow;
+ break;
+ case 3:
+ servo_id=WRIST;
+ if(HAS_WRIST) enabled = 1;
+ stored_value = target_wrist;
+ break;
+ }
+ if(enabled) {
+ if(REMOTE_USE_CURRENT_POSITION == 1) stored_value = servo.GetPosition(servo_id);
+ int adjusted = stored_value + adjust;
+ limit_reached = 0;
+ int servo_low_limit = servo.GetLowerLimit(servo_id);
+ int servo_high_limit = servo.GetUpperLimit(servo_id);
+ if(adjusted <= servo_low_limit) {
+ stored_value = servo_low_limit;
+ limit_reached = 1;
+ }
+ if(adjusted > servo_high_limit) {
+ stored_value = servo_high_limit;
+ limit_reached = 1;
+ }
+ if(limit_reached == 0) stored_value = adjusted;
+ pc.printf("{%d}",stored_value);
+ servo.SetGoal(servo_id,stored_value,1);
+ pc.printf("{-}");
+ servo.trigger();
+ switch(servo_number) {
+ case 0:
+ target_base = stored_value;
+ break;
+ case 1:
+ target_shoulder = stored_value;
+ break;
+ case 2:
+ target_elbow = stored_value;
+ break;
+ case 3:
+ target_wrist = stored_value;
+ break;
+ }
+
+ } else limit_reached = 2; //Turns off LED on inactive servos
+}
+void Remote::detect_remote()
+{
+ if (remote_sense == remote_on) {
+ if(remote_on) {
+ remote_on = 0;
+ // Remote detached
+ pc.printf("Remote detached\n");
+ display.clear_display();
+ set_led(0);
+ } else {
+ remote_on = 1;
+ // Remote attached
+ pc.printf("Remote attached\n");
+ display.clear_display();
+ display.home();
+ display.write_string("REMOTE CONTROL");
+ }
+ } else {
+ if(remote_on) detect_direction();
+ }
+}
+
+/**
+ * Set the LED on the remote
+ * param: mode 0=off 1=red 2=green 3=both
+ */
+void Remote::set_led(char mode)
+{
+ switch(mode) {
+ case 0:
+ red_led=0;
+ green_led = 0;
+ break;
+ case 1:
+ red_led = 1;
+ green_led = 0;
+ break;
+ case 2:
+ red_led = 0;
+ green_led = 1;
+ break;
+ case 3:
+ red_led = 1;
+ green_led = 1;
+ break;
+ }
+}
+
+
+/*
+ * Copyright 2017 University of York
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0
+ * Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and limitations under the License.
+ *
+ */
\ No newline at end of file