James Hilder / RobotArmController

Robot Arm Controller Library

Dependents:   PR_RobotArm lighthouse_2

robotarm.cpp@0:9f7b70e0186e, 2017-02-16 (annotated)

Committer:
jah128
Date:
Thu Feb 16 23:57:45 2017 +0000
Revision:
0:9f7b70e0186e
First commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jah128 0:9f7b70e0186e 1 /* University of York Robotics Laboratory Robot Arm Controller Board
jah128 0:9f7b70e0186e 2 *
jah128 0:9f7b70e0186e 3 * Robot Arm Controller
jah128 0:9f7b70e0186e 4 *
jah128 0:9f7b70e0186e 5 * File: robotarm.cpp
jah128 0:9f7b70e0186e 6 *
jah128 0:9f7b70e0186e 7 * (C) Dept. Electronics & Computer Science, University of York
jah128 0:9f7b70e0186e 8 *
jah128 0:9f7b70e0186e 9 * James Hilder, Alan Millard, Shuhei Miyashita, Homero Elizondo, Jon Timmis
jah128 0:9f7b70e0186e 10 *
jah128 0:9f7b70e0186e 11 * February 2017, Version 1.0
jah128 0:9f7b70e0186e 12 */
jah128 0:9f7b70e0186e 13
jah128 0:9f7b70e0186e 14 #include "robotarm.h"
jah128 0:9f7b70e0186e 15
jah128 0:9f7b70e0186e 16 Display display; //Connects to i2c(p28,p27)
jah128 0:9f7b70e0186e 17 Remote remote; //The Remote control
jah128 0:9f7b70e0186e 18 Serial pc(USBTX,USBRX); //PC Serial Interface [note there is a BlueSMIRF BT socket too....]
jah128 0:9f7b70e0186e 19 Servo servo(p9,p10); //The single-wire serial interface for the servos connects to p9 and p10
jah128 0:9f7b70e0186e 20 short target_base = BASE_ZERO;
jah128 0:9f7b70e0186e 21 short target_shoulder = SHOULDER_ZERO;
jah128 0:9f7b70e0186e 22 short target_elbow = ELBOW_ZERO;
jah128 0:9f7b70e0186e 23 short target_wrist = WRIST_ZERO;
jah128 0:9f7b70e0186e 24
jah128 0:9f7b70e0186e 25 void Robotarm::init()
jah128 0:9f7b70e0186e 26 {
jah128 0:9f7b70e0186e 27 pc.baud(115200);
jah128 0:9f7b70e0186e 28 //hardware_test();
jah128 0:9f7b70e0186e 29 pc.printf("Robot Arm Controller\nSoftware Version %1.2f\n",SOFTWARE_VERSION_CODE);
jah128 0:9f7b70e0186e 30 display.init();
jah128 0:9f7b70e0186e 31 init_servos();
jah128 0:9f7b70e0186e 32 }
jah128 0:9f7b70e0186e 33
jah128 0:9f7b70e0186e 34 void Robotarm::init_servos()
jah128 0:9f7b70e0186e 35 {
jah128 0:9f7b70e0186e 36 Timeout fail_timeout;
jah128 0:9f7b70e0186e 37 fail_timeout.attach(this,&Robotarm::fail_init,2);
jah128 0:9f7b70e0186e 38 display.clear_display();
jah128 0:9f7b70e0186e 39 wait(0.1);
jah128 0:9f7b70e0186e 40 display.set_position(0,0);
jah128 0:9f7b70e0186e 41 display.write_string("CHECKING SERVOS:");
jah128 0:9f7b70e0186e 42 pc.printf("Initialising robot arm\n");
jah128 0:9f7b70e0186e 43 wait(0.1);
jah128 0:9f7b70e0186e 44
jah128 0:9f7b70e0186e 45 if(HAS_BASE) {
jah128 0:9f7b70e0186e 46 display.set_position(1,0);
jah128 0:9f7b70e0186e 47 display.write_string("BASE:");
jah128 0:9f7b70e0186e 48 pc.printf("Check servo %d [BASE]:",BASE);
jah128 0:9f7b70e0186e 49 if(servo.GetModelNumber(BASE) != MX28_MODEL) fail_init();
jah128 0:9f7b70e0186e 50 servo.SetLowVoltageLimit(BASE, LOW_VOLTAGE_LIMIT);
jah128 0:9f7b70e0186e 51 servo.SetHighVoltageLimit(BASE, HIGH_VOLTAGE_LIMIT);
jah128 0:9f7b70e0186e 52 servo.SetTemperatureLimit(BASE, HIGH_TEMPERATURE_LIMIT);
jah128 0:9f7b70e0186e 53 //servo.SetCWLimit(BASE, 0);
jah128 0:9f7b70e0186e 54 //servo.SetCCWLimit(BASE, 0);
jah128 0:9f7b70e0186e 55 servo.LockEeprom(BASE);
jah128 0:9f7b70e0186e 56 pc.printf("OK\n");
jah128 0:9f7b70e0186e 57 display.set_position(1,9);
jah128 0:9f7b70e0186e 58 display.write_string("[OK]");
jah128 0:9f7b70e0186e 59 wait(0.2);
jah128 0:9f7b70e0186e 60 }
jah128 0:9f7b70e0186e 61
jah128 0:9f7b70e0186e 62 if(HAS_SHOULDER) {
jah128 0:9f7b70e0186e 63 display.set_position(1,0);
jah128 0:9f7b70e0186e 64 display.write_string("SHOULDER: ");
jah128 0:9f7b70e0186e 65 pc.printf("Check servo %d [SHOULDER]:",SHOULDER);
jah128 0:9f7b70e0186e 66 if(servo.GetModelNumber(SHOULDER) != MX28_MODEL) fail_init();
jah128 0:9f7b70e0186e 67 servo.SetLowVoltageLimit(SHOULDER, LOW_VOLTAGE_LIMIT);
jah128 0:9f7b70e0186e 68 servo.SetHighVoltageLimit(SHOULDER, HIGH_VOLTAGE_LIMIT);
jah128 0:9f7b70e0186e 69 servo.SetTemperatureLimit(SHOULDER, HIGH_TEMPERATURE_LIMIT);
jah128 0:9f7b70e0186e 70 //servo.SetCWLimit(SHOULDER, 0);
jah128 0:9f7b70e0186e 71 //servo.SetCCWLimit(SHOULDER, 0);
jah128 0:9f7b70e0186e 72 servo.LockEeprom(SHOULDER);
jah128 0:9f7b70e0186e 73 pc.printf("OK\n");
jah128 0:9f7b70e0186e 74 display.set_position(1,9);
jah128 0:9f7b70e0186e 75 display.write_string("[OK]");
jah128 0:9f7b70e0186e 76 wait(0.2);
jah128 0:9f7b70e0186e 77 }
jah128 0:9f7b70e0186e 78
jah128 0:9f7b70e0186e 79 if(HAS_ELBOW) {
jah128 0:9f7b70e0186e 80 display.set_position(1,0);
jah128 0:9f7b70e0186e 81 display.write_string("ELBOW: ");
jah128 0:9f7b70e0186e 82 pc.printf("Check servo %d [ELBOW]:",ELBOW);
jah128 0:9f7b70e0186e 83 if(servo.GetModelNumber(ELBOW) != MX28_MODEL) fail_init();
jah128 0:9f7b70e0186e 84 servo.SetLowVoltageLimit(ELBOW, LOW_VOLTAGE_LIMIT);
jah128 0:9f7b70e0186e 85 servo.SetHighVoltageLimit(ELBOW, HIGH_VOLTAGE_LIMIT);
jah128 0:9f7b70e0186e 86 servo.SetTemperatureLimit(ELBOW, HIGH_TEMPERATURE_LIMIT);
jah128 0:9f7b70e0186e 87 //servo.SetCWLimit(ELBOW, 0);
jah128 0:9f7b70e0186e 88 //servo.SetCCWLimit(ELBOW, 0);
jah128 0:9f7b70e0186e 89 servo.LockEeprom(ELBOW);
jah128 0:9f7b70e0186e 90 pc.printf("OK\n");
jah128 0:9f7b70e0186e 91 display.set_position(1,9);
jah128 0:9f7b70e0186e 92 display.write_string("[OK]");
jah128 0:9f7b70e0186e 93 wait(0.2);
jah128 0:9f7b70e0186e 94 }
jah128 0:9f7b70e0186e 95
jah128 0:9f7b70e0186e 96 if(HAS_WRIST) {
jah128 0:9f7b70e0186e 97 display.set_position(1,0);
jah128 0:9f7b70e0186e 98 display.write_string("WRIST: ");
jah128 0:9f7b70e0186e 99 pc.printf("Check servo %d [WRIST]:",WRIST);
jah128 0:9f7b70e0186e 100 if(servo.GetModelNumber(WRIST) != AX12_MODEL) fail_init();
jah128 0:9f7b70e0186e 101 servo.SetLowVoltageLimit(WRIST, LOW_VOLTAGE_LIMIT);
jah128 0:9f7b70e0186e 102 servo.SetHighVoltageLimit(WRIST, HIGH_VOLTAGE_LIMIT);
jah128 0:9f7b70e0186e 103 servo.SetTemperatureLimit(WRIST, HIGH_TEMPERATURE_LIMIT);
jah128 0:9f7b70e0186e 104 //servo.SetCWLimit(WRIST, 0);
jah128 0:9f7b70e0186e 105 //servo.SetCCWLimit(WRIST, 0);
jah128 0:9f7b70e0186e 106 servo.LockEeprom(WRIST);
jah128 0:9f7b70e0186e 107 pc.printf("OK\n");
jah128 0:9f7b70e0186e 108 display.set_position(1,9);
jah128 0:9f7b70e0186e 109 display.write_string("[OK]");
jah128 0:9f7b70e0186e 110 wait(0.2);
jah128 0:9f7b70e0186e 111 }
jah128 0:9f7b70e0186e 112
jah128 0:9f7b70e0186e 113 fail_timeout.detach();
jah128 0:9f7b70e0186e 114 }
jah128 0:9f7b70e0186e 115
jah128 0:9f7b70e0186e 116 float zero_time_delay;
jah128 0:9f7b70e0186e 117 Timeout zero_timeout;
jah128 0:9f7b70e0186e 118
jah128 0:9f7b70e0186e 119 void Robotarm::zero_servos(float time_delay)
jah128 0:9f7b70e0186e 120 {
jah128 0:9f7b70e0186e 121 if(time_delay <= 0)_zero_servos();
jah128 0:9f7b70e0186e 122 else {
jah128 0:9f7b70e0186e 123 zero_time_delay = time_delay;
jah128 0:9f7b70e0186e 124 display.clear_display();
jah128 0:9f7b70e0186e 125 display.set_position(0,0);
jah128 0:9f7b70e0186e 126 display.write_string("ZEROING ARM IN");
jah128 0:9f7b70e0186e 127 display.set_position(1,0);
jah128 0:9f7b70e0186e 128 char message[17];
jah128 0:9f7b70e0186e 129 sprintf(message,"%2.1f SECONDS",zero_time_delay);
jah128 0:9f7b70e0186e 130 display.write_string(message);
jah128 0:9f7b70e0186e 131 if(zero_time_delay < 0.1)zero_timeout.attach(this,&Robotarm::_zero_servos,zero_time_delay);
jah128 0:9f7b70e0186e 132 else zero_timeout.attach(this,&Robotarm::_zero_servos_display_update,0.1);
jah128 0:9f7b70e0186e 133 }
jah128 0:9f7b70e0186e 134 }
jah128 0:9f7b70e0186e 135
jah128 0:9f7b70e0186e 136 void Robotarm::_zero_servos_display_update()
jah128 0:9f7b70e0186e 137 {
jah128 0:9f7b70e0186e 138 display.set_position(1,0);
jah128 0:9f7b70e0186e 139 zero_time_delay-=0.1;
jah128 0:9f7b70e0186e 140 char message[17];
jah128 0:9f7b70e0186e 141 sprintf(message,"%2.1f SECONDS",zero_time_delay);
jah128 0:9f7b70e0186e 142 display.write_string(message);
jah128 0:9f7b70e0186e 143 if(zero_time_delay < 0.1) zero_timeout.attach(this,&Robotarm::_zero_servos,zero_time_delay);
jah128 0:9f7b70e0186e 144 else zero_timeout.attach(this,&Robotarm::_zero_servos_display_update,0.1);
jah128 0:9f7b70e0186e 145 }
jah128 0:9f7b70e0186e 146
jah128 0:9f7b70e0186e 147 void Robotarm::_zero_servos()
jah128 0:9f7b70e0186e 148 {
jah128 0:9f7b70e0186e 149 display.clear_display();
jah128 0:9f7b70e0186e 150 servo.SetGoal(BASE,BASE_ZERO,0);
jah128 0:9f7b70e0186e 151 servo.SetGoal(SHOULDER,SHOULDER_ZERO,0);
jah128 0:9f7b70e0186e 152 servo.SetGoal(ELBOW,ELBOW_ZERO,0);
jah128 0:9f7b70e0186e 153 servo.SetGoal(WRIST,WRIST_ZERO,0);
jah128 0:9f7b70e0186e 154 servo.trigger();
jah128 0:9f7b70e0186e 155 target_base = BASE_ZERO;
jah128 0:9f7b70e0186e 156 target_shoulder = SHOULDER_ZERO;
jah128 0:9f7b70e0186e 157 target_elbow = ELBOW_ZERO;
jah128 0:9f7b70e0186e 158 target_wrist = WRIST_ZERO;
jah128 0:9f7b70e0186e 159 }
jah128 0:9f7b70e0186e 160
jah128 0:9f7b70e0186e 161 void Robotarm::fail_init()
jah128 0:9f7b70e0186e 162 {
jah128 0:9f7b70e0186e 163 display.clear_display();
jah128 0:9f7b70e0186e 164 display.set_position(0,0);
jah128 0:9f7b70e0186e 165 display.write_string("SERVO ERROR");
jah128 0:9f7b70e0186e 166 while(1) {
jah128 0:9f7b70e0186e 167 remote.set_led(1);
jah128 0:9f7b70e0186e 168 wait(0.2);
jah128 0:9f7b70e0186e 169 remote.set_led(0);
jah128 0:9f7b70e0186e 170 wait(0.1);
jah128 0:9f7b70e0186e 171 }
jah128 0:9f7b70e0186e 172 }
jah128 0:9f7b70e0186e 173
jah128 0:9f7b70e0186e 174 void Robotarm::hardware_test()
jah128 0:9f7b70e0186e 175 {
jah128 0:9f7b70e0186e 176 pc.printf("\nBeginning extended hardware test\n");
jah128 0:9f7b70e0186e 177 wait(0.5);
jah128 0:9f7b70e0186e 178 pc.printf("Scanning for servos:\n");
jah128 0:9f7b70e0186e 179 servo.ScanForServos();
jah128 0:9f7b70e0186e 180 pc.printf("\nDone.");
jah128 0:9f7b70e0186e 181 while(1);
jah128 0:9f7b70e0186e 182 }
jah128 0:9f7b70e0186e 183
jah128 0:9f7b70e0186e 184 /*
jah128 0:9f7b70e0186e 185 * Copyright 2017 University of York
jah128 0:9f7b70e0186e 186 *
jah128 0:9f7b70e0186e 187 * Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License.
jah128 0:9f7b70e0186e 188 * You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0
jah128 0:9f7b70e0186e 189 * Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS
jah128 0:9f7b70e0186e 190 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
jah128 0:9f7b70e0186e 191 * See the License for the specific language governing permissions and limitations under the License.
jah128 0:9f7b70e0186e 192 *
jah128 0:9f7b70e0186e 193 */