For YRL Robot Arm
remote.cpp
- Committer:
- jah128
- Date:
- 2017-03-03
- Revision:
- 0:b14dfd8816da
File content as of revision 0:b14dfd8816da:
/* 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. * */