File content as of revision 1:5c1fc4e9401a:

//============================================================================
// Name        : piano_robot.cpp
// Author      : 
// Version     :
// Copyright   : Your copyright notice
// Description : Hello World in C++, Ansi-style
//============================================================================

#include <iostream>

#include "mbed.h"
#include "QEI.h"
#include "read_filter_emg.h"
#include "check_state.h"
#include "check_state_change.h"
#include "move_motor.h"

AnalogIn analog_emg_left(A0);
PinName pinLeft = A0;
AnalogIn analog_emg_right(A1);
PinName pinRight = A1;

Ticker emg;

//encoder for the horizontal motor
QEI horizontal_encoder (PTC10, PTC11, NC, 624); // Pin for counting (analog in)

//encoder for the keypress motor
//QEI keypress_encoder (PTC10, PTC11, NC, 624); // Pin for counting (analog in)

//initialise the variables for the horizontal motor 
DigitalOut dir_horizontal(D4); 
PwmOut pwm_horizontal(D5);
double position_horizonal = horizontal_encoder.getPulses(); //actual position of horizontal movement motor
double setpoint_horizonal = 1; //desired position, needs to be in pulses?
double Kp_horizonal = 0; //controller gain
double Ki_horizontal = 0; //set to zero

//used to pass the value fo dir_horizontal by reference to the move motor funciton
double dir_hor_ref = dir_horizontal;
double pwm_hor_ref = pwm_horizontal;

//initialise the variables for the keypress motor
DigitalOut dir_keypress(D7); //direction
PwmOut pwm_keypress(D6); //speed

//change to keypress encoder!!
double position_keypress = horizontal_encoder.getPulses(); //actual position of keypress movement motor

double setpoint_keypress = 1; //desired position, always the same actually
double Kp_keypress = 0; //controller gain, different from left and right
double Ki_keypress = 0; //set to zero

double dir_keypress_ref = dir_keypress;
double pwm_keypress_ref = pwm_keypress;
		
//initialised to the value of on threashold (value needed to turn on the system)
//check_state_change alters the value depending on if the system has switched from on to off
double threashold = 10;

//tracks whether system is left,right,keypress,rest initialised to rest
std::string state = "rest";

//used to track how state from time to next time step
std::string newState;

//used to track the amount of time muscles are contracted, or newState changes over time
int num_on_inputs = 0;

//v1 and v2 are initialised to zero, they are updated in the emg.filter() function
double v1_left = 0;
double v2_left = 0;

double v1_right = 0;
double v2_right = 0;

int main() {
	//create instance of left emg
	EMG emg_left(analog_emg_left, v1_left, v2_left);
	
	//EMG emg_left(pinLeft, v1_left, v2_left);
	
	//create instance of right emg
	EMG emg_right(analog_emg_right, v1_right, v2_right);
	
	//EMG emg_right(pinRight, v1_right, v2_right);
	
	/*THIS SECTION NEEDS TO BE CONTROLED USING TIMING*/
	while(true) 
	{
		//find value at current time
		emg_left.sample();
		//filter that value
		double input_left = emg_left.filter(emg_left.input_sample);
	
		//find value at current time
		emg_right.sample();
		//filter that value
		double input_right = emg_right.filter(emg_right.input_sample);
	
		//find which muscles are contracted above thresholds and return left,right,keypress or rest
		newState = check_state(input_left, input_right, threashold);
	
		//check whether new state = previous state and update num_on_inputs accordingly
		check_state_change(state, newState, num_on_inputs, threashold);
	
		//if num_on_inputs indicates same state for 0.25s move the necc. motor, if at rest just resets num_on_inputs
		//update(state, num_on_inputs);
		if (num_on_inputs == 250)
		{
			num_on_inputs = 0;
			if (state == "left")
			{
				move_motor(dir_horizontal, pwm_horizontal, position_horizonal, setpoint_horizonal, Kp_horizonal, Ki_horizontal);
			}
			else if (state == "right")
			{
				//same as left but setpoint is negative so it moves in the other direction
				move_motor(dir_horizontal, pwm_horizontal, position_horizonal, -setpoint_horizonal, Kp_horizonal, Ki_horizontal);
			}
			else if (state == "keypress")
			{
				move_motor(dir_keypress, pwm_keypress, position_keypress, setpoint_keypress, Kp_keypress, Ki_keypress);
				//needs to move back to starting position so recall with opposite setpoint
				move_motor(dir_keypress, pwm_keypress, position_keypress, setpoint_keypress, Kp_keypress, Ki_keypress);
			}
			//else rest do nothing
		}
	}
	return 0;
}