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?
User | Revision | Line number | New 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 | */ |