Diff: robotarm.cpp

Revision:

0:b14dfd8816da

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/robotarm.cpp	Fri Mar 03 13:28:54 2017 +0000
@@ -0,0 +1,228 @@
+/* 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;
+Serial pc(USBTX,USBRX);
+Servo servo(p9,p10);
+short target_base = 2048;
+short target_shoulder = 2048;
+short target_elbow = 2048;
+short target_wrist = 2048;
+
+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,2048,1);
+    servo.SetGoal(SHOULDER,2048,1);
+    servo.SetGoal(ELBOW,2048,1);
+    servo.trigger();
+    target_base = 2048;
+    target_shoulder = 2048;
+    target_elbow = 2048;
+    target_wrist = 2048;
+}
+
+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);
+}
+
+// Important: initialise_servo() is ONLY used when setting the servo IDs for the first time,
+// do not run once the arm has been set up!
+void Robotarm::initialise_servo(int target_servo)
+{
+    pc.printf("RUNNING SERVO SETUP ROUTINE FOR SERVO %d\n\n",target_servo);
+
+    // MX-28s default to a baud rate of 57600 with delay time of 500us
+    servo.SetInitBaud(57600, 250);
+
+    int model = servo.GetModelNumber(1);
+    if(model != MX28_MODEL) {
+        pc.printf("Error: No MX-28 servo with ID 1 found...\n");
+        while(1);
+    }
+    servo.DebugData(1);
+
+    pc.printf("\n\nSetting up MX-28 servo to ID %d\n",target_servo);
+    servo.SetID(0x01,target_servo);
+    wait(0.25);
+    //Uncomment the following block to setup servo to use 1Mbps baud rate
+    /*
+    pc.printf("Setting baud rate for servo to 1000000\n");
+    servo.SetBaud(target_servo,1);
+    wait(0.25);
+    pc.printf("Setting return delay time for servo to %dus\n",RETURN_DELAY + RETURN_DELAY);
+    servo.SetDelayTime(target_servo,RETURN_DELAY);
+    wait(0.25);
+    pc.printf("Resetting connecting baud rate\n");
+    servo.SetInitBaud(1000000, RETURN_DELAY);
+    wait(0.25);
+    */
+    pc.printf("Getting servo data:\n");
+    servo.DebugData(target_servo);
+    while(0.25);
+}
+
+/*
+ * Copyright 2016 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