Diff: robotarm.cpp

Revision:

0:9f7b70e0186e

diff -r 000000000000 -r 9f7b70e0186e robotarm.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/robotarm.cpp	Thu Feb 16 23:57:45 2017 +0000
@@ -0,0 +1,193 @@
+/* University of York Robotics Laboratory Robot Arm Controller Board
+ *
+ * Robot Arm Controller 
+ *
+ * File: robotarm.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"
+
+Display display;                    //Connects to i2c(p28,p27)
+Remote remote;                      //The Remote control
+Serial pc(USBTX,USBRX);             //PC Serial Interface [note there is a BlueSMIRF BT socket too....]
+Servo servo(p9,p10);                //The single-wire serial interface for the servos connects to p9 and p10
+short target_base = BASE_ZERO;
+short target_shoulder = SHOULDER_ZERO;
+short target_elbow = ELBOW_ZERO;
+short target_wrist = WRIST_ZERO;
+
+void Robotarm::init()
+{
+    pc.baud(115200);
+    //hardware_test();
+    pc.printf("Robot Arm Controller\nSoftware Version %1.2f\n",SOFTWARE_VERSION_CODE);
+    display.init();
+    init_servos();
+}
+
+void Robotarm::init_servos()
+{
+    Timeout fail_timeout;
+    fail_timeout.attach(this,&Robotarm::fail_init,2);
+    display.clear_display();
+    wait(0.1);
+    display.set_position(0,0);
+    display.write_string("CHECKING SERVOS:");
+    pc.printf("Initialising robot arm\n");
+    wait(0.1);
+
+    if(HAS_BASE) {
+        display.set_position(1,0);
+        display.write_string("BASE:");
+        pc.printf("Check servo %d [BASE]:",BASE);
+        if(servo.GetModelNumber(BASE) != MX28_MODEL) fail_init();
+        servo.SetLowVoltageLimit(BASE, LOW_VOLTAGE_LIMIT);
+        servo.SetHighVoltageLimit(BASE, HIGH_VOLTAGE_LIMIT);
+        servo.SetTemperatureLimit(BASE, HIGH_TEMPERATURE_LIMIT);
+        //servo.SetCWLimit(BASE, 0);
+        //servo.SetCCWLimit(BASE, 0);
+        servo.LockEeprom(BASE);
+        pc.printf("OK\n");
+        display.set_position(1,9);
+        display.write_string("[OK]");
+        wait(0.2);
+    }
+
+    if(HAS_SHOULDER) {
+        display.set_position(1,0);
+        display.write_string("SHOULDER:        ");
+        pc.printf("Check servo %d [SHOULDER]:",SHOULDER);
+        if(servo.GetModelNumber(SHOULDER) != MX28_MODEL) fail_init();
+        servo.SetLowVoltageLimit(SHOULDER, LOW_VOLTAGE_LIMIT);
+        servo.SetHighVoltageLimit(SHOULDER, HIGH_VOLTAGE_LIMIT);
+        servo.SetTemperatureLimit(SHOULDER, HIGH_TEMPERATURE_LIMIT);
+        //servo.SetCWLimit(SHOULDER, 0);
+        //servo.SetCCWLimit(SHOULDER, 0);
+        servo.LockEeprom(SHOULDER);
+        pc.printf("OK\n");
+        display.set_position(1,9);
+        display.write_string("[OK]");
+        wait(0.2);
+    }
+
+    if(HAS_ELBOW) {
+        display.set_position(1,0);
+        display.write_string("ELBOW:           ");
+        pc.printf("Check servo %d [ELBOW]:",ELBOW);
+        if(servo.GetModelNumber(ELBOW) != MX28_MODEL) fail_init();
+        servo.SetLowVoltageLimit(ELBOW, LOW_VOLTAGE_LIMIT);
+        servo.SetHighVoltageLimit(ELBOW, HIGH_VOLTAGE_LIMIT);
+        servo.SetTemperatureLimit(ELBOW, HIGH_TEMPERATURE_LIMIT);
+        //servo.SetCWLimit(ELBOW, 0);
+        //servo.SetCCWLimit(ELBOW, 0);
+        servo.LockEeprom(ELBOW);
+        pc.printf("OK\n");
+        display.set_position(1,9);
+        display.write_string("[OK]");
+        wait(0.2);
+    }
+
+    if(HAS_WRIST) {
+        display.set_position(1,0);
+        display.write_string("WRIST:          ");
+        pc.printf("Check servo %d [WRIST]:",WRIST);
+        if(servo.GetModelNumber(WRIST) != AX12_MODEL) fail_init();
+        servo.SetLowVoltageLimit(WRIST, LOW_VOLTAGE_LIMIT);
+        servo.SetHighVoltageLimit(WRIST, HIGH_VOLTAGE_LIMIT);
+        servo.SetTemperatureLimit(WRIST, HIGH_TEMPERATURE_LIMIT);
+        //servo.SetCWLimit(WRIST, 0);
+        //servo.SetCCWLimit(WRIST, 0);
+        servo.LockEeprom(WRIST);
+        pc.printf("OK\n");
+        display.set_position(1,9);
+        display.write_string("[OK]");
+        wait(0.2);
+    }
+
+    fail_timeout.detach();
+}
+
+float zero_time_delay;
+Timeout zero_timeout;
+
+void Robotarm::zero_servos(float time_delay)
+{
+    if(time_delay <= 0)_zero_servos();
+    else {
+        zero_time_delay = time_delay;
+        display.clear_display();
+        display.set_position(0,0);
+        display.write_string("ZEROING ARM IN");
+        display.set_position(1,0);
+        char message[17];
+        sprintf(message,"%2.1f SECONDS",zero_time_delay);
+        display.write_string(message);
+        if(zero_time_delay < 0.1)zero_timeout.attach(this,&Robotarm::_zero_servos,zero_time_delay);
+        else zero_timeout.attach(this,&Robotarm::_zero_servos_display_update,0.1);
+    }
+}
+
+void Robotarm::_zero_servos_display_update()
+{
+    display.set_position(1,0);
+    zero_time_delay-=0.1;
+    char message[17];
+    sprintf(message,"%2.1f SECONDS",zero_time_delay);
+    display.write_string(message);
+    if(zero_time_delay < 0.1) zero_timeout.attach(this,&Robotarm::_zero_servos,zero_time_delay);
+    else zero_timeout.attach(this,&Robotarm::_zero_servos_display_update,0.1);
+}
+
+void Robotarm::_zero_servos()
+{
+    display.clear_display();
+    servo.SetGoal(BASE,BASE_ZERO,0);
+    servo.SetGoal(SHOULDER,SHOULDER_ZERO,0);
+    servo.SetGoal(ELBOW,ELBOW_ZERO,0);
+    servo.SetGoal(WRIST,WRIST_ZERO,0);
+    servo.trigger();
+    target_base = BASE_ZERO;
+    target_shoulder = SHOULDER_ZERO;
+    target_elbow = ELBOW_ZERO;
+    target_wrist = WRIST_ZERO;
+}
+
+void Robotarm::fail_init()
+{
+    display.clear_display();
+    display.set_position(0,0);
+    display.write_string("SERVO ERROR");
+    while(1) {
+        remote.set_led(1);
+        wait(0.2);
+        remote.set_led(0);
+        wait(0.1);
+    }
+}
+
+void Robotarm::hardware_test()
+{
+    pc.printf("\nBeginning extended hardware test\n");
+    wait(0.5);
+    pc.printf("Scanning for servos:\n");
+    servo.ScanForServos();
+    pc.printf("\nDone.");
+    while(1);
+}
+
+/*
+ * 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.
+ *
+ */
\ No newline at end of file