File content as of revision 0:9f7b70e0186e:

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