Diff: main.cpp

Revision:

0:fc6fa085d591

Child:

1:5c1fc4e9401a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Oct 23 13:14:57 2015 +0000
@@ -0,0 +1,132 @@
+//============================================================================
+// 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;
+}
+