James Hilder / PR-RobotArmController

For YRL Robot Arm

robotarm.cpp@0:b14dfd8816da, 2017-03-03 (annotated)

Committer:
jah128
Date:
Fri Mar 03 13:28:54 2017 +0000
Revision:
0:b14dfd8816da
Updated

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 Controller
jah128 0:b14dfd8816da 4 *
jah128 0:b14dfd8816da 5 * File: robotarm.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 #include "robotarm.h"
jah128 0:b14dfd8816da 15
jah128 0:b14dfd8816da 16 Display display; //Connects to i2c(p28,p27)
jah128 0:b14dfd8816da 17 Remote remote;
jah128 0:b14dfd8816da 18 Serial pc(USBTX,USBRX);
jah128 0:b14dfd8816da 19 Servo servo(p9,p10);
jah128 0:b14dfd8816da 20 short target_base = 2048;
jah128 0:b14dfd8816da 21 short target_shoulder = 2048;
jah128 0:b14dfd8816da 22 short target_elbow = 2048;
jah128 0:b14dfd8816da 23 short target_wrist = 2048;
jah128 0:b14dfd8816da 24
jah128 0:b14dfd8816da 25 void Robotarm::init()
jah128 0:b14dfd8816da 26 {
jah128 0:b14dfd8816da 27 pc.baud(115200);
jah128 0:b14dfd8816da 28 //hardware_test();
jah128 0:b14dfd8816da 29 pc.printf("Robot Arm Controller\nSoftware Version %1.2f\n",SOFTWARE_VERSION_CODE);
jah128 0:b14dfd8816da 30 display.init();
jah128 0:b14dfd8816da 31 init_servos();
jah128 0:b14dfd8816da 32 }
jah128 0:b14dfd8816da 33
jah128 0:b14dfd8816da 34 void Robotarm::init_servos()
jah128 0:b14dfd8816da 35 {
jah128 0:b14dfd8816da 36 Timeout fail_timeout;
jah128 0:b14dfd8816da 37 fail_timeout.attach(this,&Robotarm::fail_init,2);
jah128 0:b14dfd8816da 38 display.clear_display();
jah128 0:b14dfd8816da 39 wait(0.1);
jah128 0:b14dfd8816da 40 display.set_position(0,0);
jah128 0:b14dfd8816da 41 display.write_string("CHECKING SERVOS:");
jah128 0:b14dfd8816da 42 pc.printf("Initialising robot arm\n");
jah128 0:b14dfd8816da 43 wait(0.1);
jah128 0:b14dfd8816da 44
jah128 0:b14dfd8816da 45 if(HAS_BASE) {
jah128 0:b14dfd8816da 46 display.set_position(1,0);
jah128 0:b14dfd8816da 47 display.write_string("BASE:");
jah128 0:b14dfd8816da 48 pc.printf("Check servo %d [BASE]:",BASE);
jah128 0:b14dfd8816da 49 if(servo.GetModelNumber(BASE) != MX28_MODEL) fail_init();
jah128 0:b14dfd8816da 50 servo.SetLowVoltageLimit(BASE, LOW_VOLTAGE_LIMIT);
jah128 0:b14dfd8816da 51 servo.SetHighVoltageLimit(BASE, HIGH_VOLTAGE_LIMIT);
jah128 0:b14dfd8816da 52 servo.SetTemperatureLimit(BASE, HIGH_TEMPERATURE_LIMIT);
jah128 0:b14dfd8816da 53 //servo.SetCWLimit(BASE, 0);
jah128 0:b14dfd8816da 54 //servo.SetCCWLimit(BASE, 0);
jah128 0:b14dfd8816da 55 servo.LockEeprom(BASE);
jah128 0:b14dfd8816da 56 pc.printf("OK\n");
jah128 0:b14dfd8816da 57 display.set_position(1,9);
jah128 0:b14dfd8816da 58 display.write_string("[OK]");
jah128 0:b14dfd8816da 59 wait(0.2);
jah128 0:b14dfd8816da 60 }
jah128 0:b14dfd8816da 61
jah128 0:b14dfd8816da 62 if(HAS_SHOULDER) {
jah128 0:b14dfd8816da 63 display.set_position(1,0);
jah128 0:b14dfd8816da 64 display.write_string("SHOULDER: ");
jah128 0:b14dfd8816da 65 pc.printf("Check servo %d [SHOULDER]:",SHOULDER);
jah128 0:b14dfd8816da 66 if(servo.GetModelNumber(SHOULDER) != MX28_MODEL) fail_init();
jah128 0:b14dfd8816da 67 servo.SetLowVoltageLimit(SHOULDER, LOW_VOLTAGE_LIMIT);
jah128 0:b14dfd8816da 68 servo.SetHighVoltageLimit(SHOULDER, HIGH_VOLTAGE_LIMIT);
jah128 0:b14dfd8816da 69 servo.SetTemperatureLimit(SHOULDER, HIGH_TEMPERATURE_LIMIT);
jah128 0:b14dfd8816da 70 //servo.SetCWLimit(SHOULDER, 0);
jah128 0:b14dfd8816da 71 //servo.SetCCWLimit(SHOULDER, 0);
jah128 0:b14dfd8816da 72 servo.LockEeprom(SHOULDER);
jah128 0:b14dfd8816da 73 pc.printf("OK\n");
jah128 0:b14dfd8816da 74 display.set_position(1,9);
jah128 0:b14dfd8816da 75 display.write_string("[OK]");
jah128 0:b14dfd8816da 76 wait(0.2);
jah128 0:b14dfd8816da 77 }
jah128 0:b14dfd8816da 78
jah128 0:b14dfd8816da 79 if(HAS_ELBOW) {
jah128 0:b14dfd8816da 80 display.set_position(1,0);
jah128 0:b14dfd8816da 81 display.write_string("ELBOW: ");
jah128 0:b14dfd8816da 82 pc.printf("Check servo %d [ELBOW]:",ELBOW);
jah128 0:b14dfd8816da 83 if(servo.GetModelNumber(ELBOW) != MX28_MODEL) fail_init();
jah128 0:b14dfd8816da 84 servo.SetLowVoltageLimit(ELBOW, LOW_VOLTAGE_LIMIT);
jah128 0:b14dfd8816da 85 servo.SetHighVoltageLimit(ELBOW, HIGH_VOLTAGE_LIMIT);
jah128 0:b14dfd8816da 86 servo.SetTemperatureLimit(ELBOW, HIGH_TEMPERATURE_LIMIT);
jah128 0:b14dfd8816da 87 //servo.SetCWLimit(ELBOW, 0);
jah128 0:b14dfd8816da 88 //servo.SetCCWLimit(ELBOW, 0);
jah128 0:b14dfd8816da 89 servo.LockEeprom(ELBOW);
jah128 0:b14dfd8816da 90 pc.printf("OK\n");
jah128 0:b14dfd8816da 91 display.set_position(1,9);
jah128 0:b14dfd8816da 92 display.write_string("[OK]");
jah128 0:b14dfd8816da 93 wait(0.2);
jah128 0:b14dfd8816da 94 }
jah128 0:b14dfd8816da 95
jah128 0:b14dfd8816da 96 if(HAS_WRIST) {
jah128 0:b14dfd8816da 97 display.set_position(1,0);
jah128 0:b14dfd8816da 98 display.write_string("WRIST: ");
jah128 0:b14dfd8816da 99 pc.printf("Check servo %d [WRIST]:",WRIST);
jah128 0:b14dfd8816da 100 if(servo.GetModelNumber(WRIST) != AX12_MODEL) fail_init();
jah128 0:b14dfd8816da 101 servo.SetLowVoltageLimit(WRIST, LOW_VOLTAGE_LIMIT);
jah128 0:b14dfd8816da 102 servo.SetHighVoltageLimit(WRIST, HIGH_VOLTAGE_LIMIT);
jah128 0:b14dfd8816da 103 servo.SetTemperatureLimit(WRIST, HIGH_TEMPERATURE_LIMIT);
jah128 0:b14dfd8816da 104 //servo.SetCWLimit(WRIST, 0);
jah128 0:b14dfd8816da 105 //servo.SetCCWLimit(WRIST, 0);
jah128 0:b14dfd8816da 106 servo.LockEeprom(WRIST);
jah128 0:b14dfd8816da 107 pc.printf("OK\n");
jah128 0:b14dfd8816da 108 display.set_position(1,9);
jah128 0:b14dfd8816da 109 display.write_string("[OK]");
jah128 0:b14dfd8816da 110 wait(0.2);
jah128 0:b14dfd8816da 111 }
jah128 0:b14dfd8816da 112
jah128 0:b14dfd8816da 113 fail_timeout.detach();
jah128 0:b14dfd8816da 114 }
jah128 0:b14dfd8816da 115
jah128 0:b14dfd8816da 116 float zero_time_delay;
jah128 0:b14dfd8816da 117 Timeout zero_timeout;
jah128 0:b14dfd8816da 118
jah128 0:b14dfd8816da 119 void Robotarm::zero_servos(float time_delay)
jah128 0:b14dfd8816da 120 {
jah128 0:b14dfd8816da 121 if(time_delay <= 0)_zero_servos();
jah128 0:b14dfd8816da 122 else {
jah128 0:b14dfd8816da 123 zero_time_delay = time_delay;
jah128 0:b14dfd8816da 124 display.clear_display();
jah128 0:b14dfd8816da 125 display.set_position(0,0);
jah128 0:b14dfd8816da 126 display.write_string("ZEROING ARM IN");
jah128 0:b14dfd8816da 127 display.set_position(1,0);
jah128 0:b14dfd8816da 128 char message[17];
jah128 0:b14dfd8816da 129 sprintf(message,"%2.1f SECONDS",zero_time_delay);
jah128 0:b14dfd8816da 130 display.write_string(message);
jah128 0:b14dfd8816da 131 if(zero_time_delay < 0.1)zero_timeout.attach(this,&Robotarm::_zero_servos,zero_time_delay);
jah128 0:b14dfd8816da 132 else zero_timeout.attach(this,&Robotarm::_zero_servos_display_update,0.1);
jah128 0:b14dfd8816da 133 }
jah128 0:b14dfd8816da 134 }
jah128 0:b14dfd8816da 135
jah128 0:b14dfd8816da 136 void Robotarm::_zero_servos_display_update()
jah128 0:b14dfd8816da 137 {
jah128 0:b14dfd8816da 138 display.set_position(1,0);
jah128 0:b14dfd8816da 139 zero_time_delay-=0.1;
jah128 0:b14dfd8816da 140 char message[17];
jah128 0:b14dfd8816da 141 sprintf(message,"%2.1f SECONDS",zero_time_delay);
jah128 0:b14dfd8816da 142 display.write_string(message);
jah128 0:b14dfd8816da 143 if(zero_time_delay < 0.1) zero_timeout.attach(this,&Robotarm::_zero_servos,zero_time_delay);
jah128 0:b14dfd8816da 144 else zero_timeout.attach(this,&Robotarm::_zero_servos_display_update,0.1);
jah128 0:b14dfd8816da 145 }
jah128 0:b14dfd8816da 146
jah128 0:b14dfd8816da 147 void Robotarm::_zero_servos()
jah128 0:b14dfd8816da 148 {
jah128 0:b14dfd8816da 149 display.clear_display();
jah128 0:b14dfd8816da 150 servo.SetGoal(BASE,2048,1);
jah128 0:b14dfd8816da 151 servo.SetGoal(SHOULDER,2048,1);
jah128 0:b14dfd8816da 152 servo.SetGoal(ELBOW,2048,1);
jah128 0:b14dfd8816da 153 servo.trigger();
jah128 0:b14dfd8816da 154 target_base = 2048;
jah128 0:b14dfd8816da 155 target_shoulder = 2048;
jah128 0:b14dfd8816da 156 target_elbow = 2048;
jah128 0:b14dfd8816da 157 target_wrist = 2048;
jah128 0:b14dfd8816da 158 }
jah128 0:b14dfd8816da 159
jah128 0:b14dfd8816da 160 void Robotarm::fail_init()
jah128 0:b14dfd8816da 161 {
jah128 0:b14dfd8816da 162 display.clear_display();
jah128 0:b14dfd8816da 163 display.set_position(0,0);
jah128 0:b14dfd8816da 164 display.write_string("SERVO ERROR");
jah128 0:b14dfd8816da 165 while(1) {
jah128 0:b14dfd8816da 166 remote.set_led(1);
jah128 0:b14dfd8816da 167 wait(0.2);
jah128 0:b14dfd8816da 168 remote.set_led(0);
jah128 0:b14dfd8816da 169 wait(0.1);
jah128 0:b14dfd8816da 170 }
jah128 0:b14dfd8816da 171 }
jah128 0:b14dfd8816da 172
jah128 0:b14dfd8816da 173 void Robotarm::hardware_test()
jah128 0:b14dfd8816da 174 {
jah128 0:b14dfd8816da 175 pc.printf("\nBeginning extended hardware test\n");
jah128 0:b14dfd8816da 176 wait(0.5);
jah128 0:b14dfd8816da 177 pc.printf("Scanning for servos:\n");
jah128 0:b14dfd8816da 178 servo.ScanForServos();
jah128 0:b14dfd8816da 179 pc.printf("\nDone.");
jah128 0:b14dfd8816da 180 while(1);
jah128 0:b14dfd8816da 181 }
jah128 0:b14dfd8816da 182
jah128 0:b14dfd8816da 183 // Important: initialise_servo() is ONLY used when setting the servo IDs for the first time,
jah128 0:b14dfd8816da 184 // do not run once the arm has been set up!
jah128 0:b14dfd8816da 185 void Robotarm::initialise_servo(int target_servo)
jah128 0:b14dfd8816da 186 {
jah128 0:b14dfd8816da 187 pc.printf("RUNNING SERVO SETUP ROUTINE FOR SERVO %d\n\n",target_servo);
jah128 0:b14dfd8816da 188
jah128 0:b14dfd8816da 189 // MX-28s default to a baud rate of 57600 with delay time of 500us
jah128 0:b14dfd8816da 190 servo.SetInitBaud(57600, 250);
jah128 0:b14dfd8816da 191
jah128 0:b14dfd8816da 192 int model = servo.GetModelNumber(1);
jah128 0:b14dfd8816da 193 if(model != MX28_MODEL) {
jah128 0:b14dfd8816da 194 pc.printf("Error: No MX-28 servo with ID 1 found...\n");
jah128 0:b14dfd8816da 195 while(1);
jah128 0:b14dfd8816da 196 }
jah128 0:b14dfd8816da 197 servo.DebugData(1);
jah128 0:b14dfd8816da 198
jah128 0:b14dfd8816da 199 pc.printf("\n\nSetting up MX-28 servo to ID %d\n",target_servo);
jah128 0:b14dfd8816da 200 servo.SetID(0x01,target_servo);
jah128 0:b14dfd8816da 201 wait(0.25);
jah128 0:b14dfd8816da 202 //Uncomment the following block to setup servo to use 1Mbps baud rate
jah128 0:b14dfd8816da 203 /*
jah128 0:b14dfd8816da 204 pc.printf("Setting baud rate for servo to 1000000\n");
jah128 0:b14dfd8816da 205 servo.SetBaud(target_servo,1);
jah128 0:b14dfd8816da 206 wait(0.25);
jah128 0:b14dfd8816da 207 pc.printf("Setting return delay time for servo to %dus\n",RETURN_DELAY + RETURN_DELAY);
jah128 0:b14dfd8816da 208 servo.SetDelayTime(target_servo,RETURN_DELAY);
jah128 0:b14dfd8816da 209 wait(0.25);
jah128 0:b14dfd8816da 210 pc.printf("Resetting connecting baud rate\n");
jah128 0:b14dfd8816da 211 servo.SetInitBaud(1000000, RETURN_DELAY);
jah128 0:b14dfd8816da 212 wait(0.25);
jah128 0:b14dfd8816da 213 */
jah128 0:b14dfd8816da 214 pc.printf("Getting servo data:\n");
jah128 0:b14dfd8816da 215 servo.DebugData(target_servo);
jah128 0:b14dfd8816da 216 while(0.25);
jah128 0:b14dfd8816da 217 }
jah128 0:b14dfd8816da 218
jah128 0:b14dfd8816da 219 /*
jah128 0:b14dfd8816da 220 * Copyright 2016 University of York
jah128 0:b14dfd8816da 221 *
jah128 0:b14dfd8816da 222 * 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 223 * You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0
jah128 0:b14dfd8816da 224 * 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 225 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
jah128 0:b14dfd8816da 226 * See the License for the specific language governing permissions and limitations under the License.
jah128 0:b14dfd8816da 227 *
jah128 0:b14dfd8816da 228 */