File content as of revision 1:aa92ba95a4bb:

/* University of York Robotics Laboratory Robot Arm Controller Board
 * 
 * Robot Arm Remote Control
 *
 * File: remote.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"

DigitalIn remote_sense(p17,PullUp);
DigitalIn sw1_up(p21,PullUp);
DigitalIn sw1_down(p22,PullUp);
DigitalIn sw1_left(p23,PullUp);
DigitalIn sw1_right(p24,PullUp);
DigitalIn sw2_up(p25,PullUp);
DigitalIn sw2_down(p26,PullUp);
DigitalIn sw2_left(p29,PullUp);
DigitalIn sw2_right(p30,PullUp);

DigitalOut red_led(p16);
DigitalOut green_led(p20);
Ticker attach_remote_sensor;
char remote_on = 0;
char h_switch = 0;
char limit_reached = 0;


void Remote::init()
{
    attach_remote_sensor.attach_us(this,&Remote::detect_remote,REMOTE_POLL_RATE);
}

char step = REMOTE_START_SPEED;

void Remote::detect_direction()
{
    char t_switch = 0;
    if(!sw1_up)t_switch+=1;
    if(!sw1_down)t_switch+=2;
    if(!sw1_left)t_switch+=4;
    if(!sw1_right)t_switch+=8;
    if(!sw2_up)t_switch+=16;
    if(!sw2_down)t_switch+=32;
    if(!sw2_left)t_switch+=64;
    if(!sw2_right)t_switch+=128;

    if(t_switch!=h_switch) {
        h_switch=t_switch;
        pc.printf("Switch ");
        if(h_switch == 0) {
            pc.printf("released");
            step = REMOTE_START_SPEED;
            set_led(0);
        } else {
            if(h_switch & 0x01) {
                pc.printf("1-UP ");
            }
            if(h_switch & 0x02) {
                pc.printf("1-DN ");
            }
            if(h_switch & 0x04) {
                pc.printf("1-LF ");
            }
            if(h_switch & 0x08) {
                pc.printf("1-RT ");
            }
            if(h_switch & 0x10) {
                pc.printf("2-UP ");
            }
            if(h_switch & 0x20) {
                pc.printf("2-DN ");
            }
            if(h_switch & 0x40) {
                pc.printf("2-LF ");
            }
            if(h_switch & 0x80) {
                pc.printf("2-RT ");
            }

        }
        pc.printf("\n");
    }
    if(t_switch != 0) {

        if(h_switch & 0x01) {
            move_servo(1,step);
        }
        if(h_switch & 0x02) {
            move_servo(1,-step);
        }
        if(h_switch & 0x04) {
            move_servo(0,-step);
        }
        if(h_switch & 0x08) {
            move_servo(0,step);
        }
        if(h_switch & 0x10) {
            move_servo(2,step);
        }
        if(h_switch & 0x20) {
            move_servo(2,-step);
        }
        if(h_switch & 0x40) {
            move_servo(3,-step);
        }
        if(h_switch & 0x80) {
            move_servo(3,step);
        }
        if(REMOTE_LINEAR_STEPS != 1) {
            step += REMOTE_START_SPEED;
            if(step > 200) step = 200;
        }
        set_led(2-limit_reached);
    }
}

void Remote::move_servo(char servo_number, int adjust)
{
    char enabled = 0;
    char servo_id = 0;
    short stored_value = 0;
    switch(servo_number) {
        case 0:
            servo_id=BASE;
            if(HAS_BASE)enabled = 1;
            stored_value = target_base;
            break;
        case 1:
            servo_id=SHOULDER;
            if(HAS_SHOULDER) enabled = 1;
            stored_value = target_shoulder;
            break;
        case 2:
            servo_id=ELBOW;
            if(HAS_ELBOW) enabled = 1;
            stored_value = target_elbow;
            break;
        case 3:
            servo_id=WRIST;
            if(HAS_WRIST) enabled = 1;
            stored_value = target_wrist;
            break;
    }
    if(enabled) {
        if(REMOTE_USE_CURRENT_POSITION == 1) stored_value = servo.GetPosition(servo_id);
        int adjusted = stored_value + adjust;
        limit_reached = 0;
        int servo_low_limit = servo.GetLowerLimit(servo_id);
        int servo_high_limit = servo.GetUpperLimit(servo_id);
        if(adjusted <= servo_low_limit) {
            stored_value = servo_low_limit;
            limit_reached = 1;
        }
        if(adjusted > servo_high_limit) {
            stored_value = servo_high_limit;
            limit_reached = 1;
        }
        if(limit_reached == 0) stored_value = adjusted;
        pc.printf("{%d}",stored_value);
        servo.SetGoal(servo_id,stored_value,1);
        pc.printf("{-}");
        servo.trigger();
        switch(servo_number) {
        case 0:
            target_base = stored_value;
            break;
        case 1:
            target_shoulder = stored_value;
            break;
        case 2:
            target_elbow = stored_value;
            break;
        case 3:
            target_wrist = stored_value;
            break;
    }

    } else limit_reached = 2; //Turns off LED on inactive servos
}
void Remote::detect_remote()
{
    if (remote_sense == remote_on) {
        if(remote_on) {
            remote_on = 0;
            // Remote detached
            pc.printf("Remote detached\n");
            display.clear_display();
            set_led(0);
        } else {
            remote_on = 1;
            // Remote attached
            pc.printf("Remote attached\n");
            display.clear_display();
            display.home();
            display.write_string("REMOTE CONTROL");
        }
    } else {
        if(remote_on) detect_direction();
    }
}

/**
 * Set the LED on the remote
 * param: mode 0=off 1=red 2=green 3=both
 */
void Remote::set_led(char mode)
{
    switch(mode) {
        case 0:
            red_led=0;
            green_led = 0;
            break;
        case 1:
            red_led = 1;
            green_led = 0;
            break;
        case 2:
            red_led = 0;
            green_led = 1;
            break;
        case 3:
            red_led = 1;
            green_led = 1;
            break;
    }
}


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