James Hilder / PR_RobotArmController

For YRL Robot Arm

Dependents:   PR_RobotArm_Show_Positions

Fork of PR-RobotArmController by James Hilder

remote.cpp@1:aa92ba95a4bb, 2017-03-03 (annotated)

Committer:
jah128
Date:
Fri Mar 03 13:37:44 2017 +0000
Revision:
1:aa92ba95a4bb
Parent:
0:b14dfd8816da 
fixed wrist bug;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jah128 0:b14dfd8816da 1 /* University of York Robotics Laboratory Robot Arm Controller Board
jah128 0:b14dfd8816da 2 *
jah128 0:b14dfd8816da 3 * Robot Arm Remote Control
jah128 0:b14dfd8816da 4 *
jah128 0:b14dfd8816da 5 * File: remote.cpp
jah128 0:b14dfd8816da 6 *
jah128 0:b14dfd8816da 7 * (C) Dept. Electronics & Computer Science, University of York
jah128 0:b14dfd8816da 8 *
jah128 0:b14dfd8816da 9 * James Hilder, Alan Millard, Shuhei Miyashita, Homero Elizondo, Jon Timmis
jah128 0:b14dfd8816da 10 *
jah128 0:b14dfd8816da 11 * February 2017, Version 1.0
jah128 0:b14dfd8816da 12 *
jah128 0:b14dfd8816da 13 */
jah128 0:b14dfd8816da 14
jah128 0:b14dfd8816da 15 #include "robotarm.h"
jah128 0:b14dfd8816da 16
jah128 0:b14dfd8816da 17 DigitalIn remote_sense(p17,PullUp);
jah128 0:b14dfd8816da 18 DigitalIn sw1_up(p21,PullUp);
jah128 0:b14dfd8816da 19 DigitalIn sw1_down(p22,PullUp);
jah128 0:b14dfd8816da 20 DigitalIn sw1_left(p23,PullUp);
jah128 0:b14dfd8816da 21 DigitalIn sw1_right(p24,PullUp);
jah128 0:b14dfd8816da 22 DigitalIn sw2_up(p25,PullUp);
jah128 0:b14dfd8816da 23 DigitalIn sw2_down(p26,PullUp);
jah128 0:b14dfd8816da 24 DigitalIn sw2_left(p29,PullUp);
jah128 0:b14dfd8816da 25 DigitalIn sw2_right(p30,PullUp);
jah128 0:b14dfd8816da 26
jah128 0:b14dfd8816da 27 DigitalOut red_led(p16);
jah128 0:b14dfd8816da 28 DigitalOut green_led(p20);
jah128 0:b14dfd8816da 29 Ticker attach_remote_sensor;
jah128 0:b14dfd8816da 30 char remote_on = 0;
jah128 0:b14dfd8816da 31 char h_switch = 0;
jah128 0:b14dfd8816da 32 char limit_reached = 0;
jah128 0:b14dfd8816da 33
jah128 0:b14dfd8816da 34
jah128 0:b14dfd8816da 35 void Remote::init()
jah128 0:b14dfd8816da 36 {
jah128 0:b14dfd8816da 37 attach_remote_sensor.attach_us(this,&Remote::detect_remote,REMOTE_POLL_RATE);
jah128 0:b14dfd8816da 38 }
jah128 0:b14dfd8816da 39
jah128 0:b14dfd8816da 40 char step = REMOTE_START_SPEED;
jah128 0:b14dfd8816da 41
jah128 0:b14dfd8816da 42 void Remote::detect_direction()
jah128 0:b14dfd8816da 43 {
jah128 0:b14dfd8816da 44 char t_switch = 0;
jah128 0:b14dfd8816da 45 if(!sw1_up)t_switch+=1;
jah128 0:b14dfd8816da 46 if(!sw1_down)t_switch+=2;
jah128 0:b14dfd8816da 47 if(!sw1_left)t_switch+=4;
jah128 0:b14dfd8816da 48 if(!sw1_right)t_switch+=8;
jah128 0:b14dfd8816da 49 if(!sw2_up)t_switch+=16;
jah128 0:b14dfd8816da 50 if(!sw2_down)t_switch+=32;
jah128 0:b14dfd8816da 51 if(!sw2_left)t_switch+=64;
jah128 0:b14dfd8816da 52 if(!sw2_right)t_switch+=128;
jah128 0:b14dfd8816da 53
jah128 0:b14dfd8816da 54 if(t_switch!=h_switch) {
jah128 0:b14dfd8816da 55 h_switch=t_switch;
jah128 0:b14dfd8816da 56 pc.printf("Switch ");
jah128 0:b14dfd8816da 57 if(h_switch == 0) {
jah128 0:b14dfd8816da 58 pc.printf("released");
jah128 0:b14dfd8816da 59 step = REMOTE_START_SPEED;
jah128 0:b14dfd8816da 60 set_led(0);
jah128 0:b14dfd8816da 61 } else {
jah128 0:b14dfd8816da 62 if(h_switch & 0x01) {
jah128 0:b14dfd8816da 63 pc.printf("1-UP ");
jah128 0:b14dfd8816da 64 }
jah128 0:b14dfd8816da 65 if(h_switch & 0x02) {
jah128 0:b14dfd8816da 66 pc.printf("1-DN ");
jah128 0:b14dfd8816da 67 }
jah128 0:b14dfd8816da 68 if(h_switch & 0x04) {
jah128 0:b14dfd8816da 69 pc.printf("1-LF ");
jah128 0:b14dfd8816da 70 }
jah128 0:b14dfd8816da 71 if(h_switch & 0x08) {
jah128 0:b14dfd8816da 72 pc.printf("1-RT ");
jah128 0:b14dfd8816da 73 }
jah128 0:b14dfd8816da 74 if(h_switch & 0x10) {
jah128 0:b14dfd8816da 75 pc.printf("2-UP ");
jah128 0:b14dfd8816da 76 }
jah128 0:b14dfd8816da 77 if(h_switch & 0x20) {
jah128 0:b14dfd8816da 78 pc.printf("2-DN ");
jah128 0:b14dfd8816da 79 }
jah128 0:b14dfd8816da 80 if(h_switch & 0x40) {
jah128 0:b14dfd8816da 81 pc.printf("2-LF ");
jah128 0:b14dfd8816da 82 }
jah128 0:b14dfd8816da 83 if(h_switch & 0x80) {
jah128 0:b14dfd8816da 84 pc.printf("2-RT ");
jah128 0:b14dfd8816da 85 }
jah128 0:b14dfd8816da 86
jah128 0:b14dfd8816da 87 }
jah128 0:b14dfd8816da 88 pc.printf("\n");
jah128 0:b14dfd8816da 89 }
jah128 0:b14dfd8816da 90 if(t_switch != 0) {
jah128 0:b14dfd8816da 91
jah128 0:b14dfd8816da 92 if(h_switch & 0x01) {
jah128 0:b14dfd8816da 93 move_servo(1,step);
jah128 0:b14dfd8816da 94 }
jah128 0:b14dfd8816da 95 if(h_switch & 0x02) {
jah128 0:b14dfd8816da 96 move_servo(1,-step);
jah128 0:b14dfd8816da 97 }
jah128 0:b14dfd8816da 98 if(h_switch & 0x04) {
jah128 0:b14dfd8816da 99 move_servo(0,-step);
jah128 0:b14dfd8816da 100 }
jah128 0:b14dfd8816da 101 if(h_switch & 0x08) {
jah128 0:b14dfd8816da 102 move_servo(0,step);
jah128 0:b14dfd8816da 103 }
jah128 0:b14dfd8816da 104 if(h_switch & 0x10) {
jah128 0:b14dfd8816da 105 move_servo(2,step);
jah128 0:b14dfd8816da 106 }
jah128 0:b14dfd8816da 107 if(h_switch & 0x20) {
jah128 0:b14dfd8816da 108 move_servo(2,-step);
jah128 0:b14dfd8816da 109 }
jah128 0:b14dfd8816da 110 if(h_switch & 0x40) {
jah128 0:b14dfd8816da 111 move_servo(3,-step);
jah128 0:b14dfd8816da 112 }
jah128 0:b14dfd8816da 113 if(h_switch & 0x80) {
jah128 0:b14dfd8816da 114 move_servo(3,step);
jah128 0:b14dfd8816da 115 }
jah128 0:b14dfd8816da 116 if(REMOTE_LINEAR_STEPS != 1) {
jah128 0:b14dfd8816da 117 step += REMOTE_START_SPEED;
jah128 0:b14dfd8816da 118 if(step > 200) step = 200;
jah128 0:b14dfd8816da 119 }
jah128 0:b14dfd8816da 120 set_led(2-limit_reached);
jah128 0:b14dfd8816da 121 }
jah128 0:b14dfd8816da 122 }
jah128 0:b14dfd8816da 123
jah128 0:b14dfd8816da 124 void Remote::move_servo(char servo_number, int adjust)
jah128 0:b14dfd8816da 125 {
jah128 0:b14dfd8816da 126 char enabled = 0;
jah128 0:b14dfd8816da 127 char servo_id = 0;
jah128 0:b14dfd8816da 128 short stored_value = 0;
jah128 0:b14dfd8816da 129 switch(servo_number) {
jah128 0:b14dfd8816da 130 case 0:
jah128 0:b14dfd8816da 131 servo_id=BASE;
jah128 0:b14dfd8816da 132 if(HAS_BASE)enabled = 1;
jah128 0:b14dfd8816da 133 stored_value = target_base;
jah128 0:b14dfd8816da 134 break;
jah128 0:b14dfd8816da 135 case 1:
jah128 0:b14dfd8816da 136 servo_id=SHOULDER;
jah128 0:b14dfd8816da 137 if(HAS_SHOULDER) enabled = 1;
jah128 0:b14dfd8816da 138 stored_value = target_shoulder;
jah128 0:b14dfd8816da 139 break;
jah128 0:b14dfd8816da 140 case 2:
jah128 0:b14dfd8816da 141 servo_id=ELBOW;
jah128 0:b14dfd8816da 142 if(HAS_ELBOW) enabled = 1;
jah128 0:b14dfd8816da 143 stored_value = target_elbow;
jah128 0:b14dfd8816da 144 break;
jah128 0:b14dfd8816da 145 case 3:
jah128 0:b14dfd8816da 146 servo_id=WRIST;
jah128 0:b14dfd8816da 147 if(HAS_WRIST) enabled = 1;
jah128 0:b14dfd8816da 148 stored_value = target_wrist;
jah128 0:b14dfd8816da 149 break;
jah128 0:b14dfd8816da 150 }
jah128 0:b14dfd8816da 151 if(enabled) {
jah128 0:b14dfd8816da 152 if(REMOTE_USE_CURRENT_POSITION == 1) stored_value = servo.GetPosition(servo_id);
jah128 0:b14dfd8816da 153 int adjusted = stored_value + adjust;
jah128 0:b14dfd8816da 154 limit_reached = 0;
jah128 0:b14dfd8816da 155 int servo_low_limit = servo.GetLowerLimit(servo_id);
jah128 0:b14dfd8816da 156 int servo_high_limit = servo.GetUpperLimit(servo_id);
jah128 0:b14dfd8816da 157 if(adjusted <= servo_low_limit) {
jah128 0:b14dfd8816da 158 stored_value = servo_low_limit;
jah128 0:b14dfd8816da 159 limit_reached = 1;
jah128 0:b14dfd8816da 160 }
jah128 0:b14dfd8816da 161 if(adjusted > servo_high_limit) {
jah128 0:b14dfd8816da 162 stored_value = servo_high_limit;
jah128 0:b14dfd8816da 163 limit_reached = 1;
jah128 0:b14dfd8816da 164 }
jah128 0:b14dfd8816da 165 if(limit_reached == 0) stored_value = adjusted;
jah128 0:b14dfd8816da 166 pc.printf("{%d}",stored_value);
jah128 0:b14dfd8816da 167 servo.SetGoal(servo_id,stored_value,1);
jah128 0:b14dfd8816da 168 pc.printf("{-}");
jah128 0:b14dfd8816da 169 servo.trigger();
jah128 0:b14dfd8816da 170 switch(servo_number) {
jah128 0:b14dfd8816da 171 case 0:
jah128 0:b14dfd8816da 172 target_base = stored_value;
jah128 0:b14dfd8816da 173 break;
jah128 0:b14dfd8816da 174 case 1:
jah128 0:b14dfd8816da 175 target_shoulder = stored_value;
jah128 0:b14dfd8816da 176 break;
jah128 0:b14dfd8816da 177 case 2:
jah128 0:b14dfd8816da 178 target_elbow = stored_value;
jah128 0:b14dfd8816da 179 break;
jah128 0:b14dfd8816da 180 case 3:
jah128 0:b14dfd8816da 181 target_wrist = stored_value;
jah128 0:b14dfd8816da 182 break;
jah128 0:b14dfd8816da 183 }
jah128 0:b14dfd8816da 184
jah128 0:b14dfd8816da 185 } else limit_reached = 2; //Turns off LED on inactive servos
jah128 0:b14dfd8816da 186 }
jah128 0:b14dfd8816da 187 void Remote::detect_remote()
jah128 0:b14dfd8816da 188 {
jah128 0:b14dfd8816da 189 if (remote_sense == remote_on) {
jah128 0:b14dfd8816da 190 if(remote_on) {
jah128 0:b14dfd8816da 191 remote_on = 0;
jah128 0:b14dfd8816da 192 // Remote detached
jah128 0:b14dfd8816da 193 pc.printf("Remote detached\n");
jah128 0:b14dfd8816da 194 display.clear_display();
jah128 0:b14dfd8816da 195 set_led(0);
jah128 0:b14dfd8816da 196 } else {
jah128 0:b14dfd8816da 197 remote_on = 1;
jah128 0:b14dfd8816da 198 // Remote attached
jah128 0:b14dfd8816da 199 pc.printf("Remote attached\n");
jah128 0:b14dfd8816da 200 display.clear_display();
jah128 0:b14dfd8816da 201 display.home();
jah128 0:b14dfd8816da 202 display.write_string("REMOTE CONTROL");
jah128 0:b14dfd8816da 203 }
jah128 0:b14dfd8816da 204 } else {
jah128 0:b14dfd8816da 205 if(remote_on) detect_direction();
jah128 0:b14dfd8816da 206 }
jah128 0:b14dfd8816da 207 }
jah128 0:b14dfd8816da 208
jah128 0:b14dfd8816da 209 /**
jah128 0:b14dfd8816da 210 * Set the LED on the remote
jah128 0:b14dfd8816da 211 * param: mode 0=off 1=red 2=green 3=both
jah128 0:b14dfd8816da 212 */
jah128 0:b14dfd8816da 213 void Remote::set_led(char mode)
jah128 0:b14dfd8816da 214 {
jah128 0:b14dfd8816da 215 switch(mode) {
jah128 0:b14dfd8816da 216 case 0:
jah128 0:b14dfd8816da 217 red_led=0;
jah128 0:b14dfd8816da 218 green_led = 0;
jah128 0:b14dfd8816da 219 break;
jah128 0:b14dfd8816da 220 case 1:
jah128 0:b14dfd8816da 221 red_led = 1;
jah128 0:b14dfd8816da 222 green_led = 0;
jah128 0:b14dfd8816da 223 break;
jah128 0:b14dfd8816da 224 case 2:
jah128 0:b14dfd8816da 225 red_led = 0;
jah128 0:b14dfd8816da 226 green_led = 1;
jah128 0:b14dfd8816da 227 break;
jah128 0:b14dfd8816da 228 case 3:
jah128 0:b14dfd8816da 229 red_led = 1;
jah128 0:b14dfd8816da 230 green_led = 1;
jah128 0:b14dfd8816da 231 break;
jah128 0:b14dfd8816da 232 }
jah128 0:b14dfd8816da 233 }
jah128 0:b14dfd8816da 234
jah128 0:b14dfd8816da 235
jah128 0:b14dfd8816da 236 /*
jah128 0:b14dfd8816da 237 * Copyright 2017 University of York
jah128 0:b14dfd8816da 238 *
jah128 0:b14dfd8816da 239 * Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License.
jah128 0:b14dfd8816da 240 * You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0
jah128 0:b14dfd8816da 241 * Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS
jah128 0:b14dfd8816da 242 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
jah128 0:b14dfd8816da 243 * See the License for the specific language governing permissions and limitations under the License.
jah128 0:b14dfd8816da 244 *
jah128 0:b14dfd8816da 245 */