File content as of revision 0:b14dfd8816da:

/* 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.
 *
 */