Diff: buttons.cpp

Revision:

57:43f707648f2b

Parent:

54:c14c3bc48b8a

Child:

59:fe00be2cf8fd

--- a/buttons.cpp	Fri Oct 09 13:06:14 2015 +0000
+++ b/buttons.cpp	Mon Oct 12 11:46:47 2015 +0200
@@ -2,6 +2,8 @@
 #include "mbed.h"
 #include "config.h"
 #include "actuators.h"
+#include "PID.h"
+
 // functions for reading all the buttons and switches 
 AnalogIn pot2(pot2Pin);
 AnalogIn pot1(pot1Pin);
@@ -20,6 +22,7 @@
 bool button1Pressed = false;
 bool button2Pressed = false;
 int actuatorState = 0;
+int PIDparam = 0;
 
 float pot1Val = 0;
 float pot2Val = 0;
@@ -27,11 +30,12 @@
 
 void checkSwitches(){
 
-	// 3 states: X control, Y control en Servo control
-	//	 button 1 to enable/disable actuators
-	// 	 button 2 to switch between states
+	// read potmeter values
+    pot1Val = pot1.read();
+    pot2Val = pot2.read();
 
-	if(button1.read() == 0){
+    // button 1 pressed
+    if(button1.read() == 0){
 		if (button1Pressed != true){
 			motorsEnable = !motorsEnable;
 			button1Pressed = true;
@@ -39,6 +43,55 @@
 	}else{
 		button1Pressed = false;
 	}
+
+	#ifdef TUNEPID
+		setForPID(); // if TUNEPID is defined, use potmeters for PID tuning
+	#elif
+		setForPositions(); // else use it for motor control
+	#endif
+}
+
+void setForPID(){
+	if(button2.read() == 0){
+		if(button2Pressed != true){
+			PIDparam++;
+			if(PIDparam==3){
+				PIDparam = 0;
+			}
+		button2Pressed = true;
+		}
+	}else{
+		button2Pressed = false;
+	}
+
+	if(motorsEnable){
+		motor2SetSpeed = 300*pot2.read();
+    	switch (PIDparam){  
+        	case 0: 				// potmeters control P gain
+        		redLed.write(0); greenLed.write(1); blueLed.write(1);
+        		float Kp = pot2.read();
+        		break;
+        	case 1:         		// potmeters control I gain
+        		redLed.write(1); greenLed.write(0); blueLed.write(1);
+        		float Ki = pot2.read();
+        		break;
+        	case 2:         		// potmeters control D gain
+        		redLed.write(1); greenLed.write(1); blueLed.write(0);
+        		float Kd = pot2.read();
+        		break;
+        }
+        motor2PID.SetTunings(Kp, Ki, Kd);
+    }else{
+    	redLed.write(1); greenLed.write(1); blueLed.write(1);
+    }
+}
+
+void setForPositions(){
+		// 3 states: X control, Y control en Servo control
+	//	 button 1 to enable/disable actuators
+	// 	 button 2 to switch between states
+
+
 	if(button2.read() == 0){
 		if(button2Pressed != true){
 			actuatorState++;
@@ -51,8 +104,7 @@
 		button2Pressed = false;
 	}
 
-    pot1Val = pot1.read();
-    pot2Val = pot2.read();
+
 
     if(motorsEnable){
     	switch (actuatorState){
@@ -72,4 +124,4 @@
     }else{
     	redLed.write(1); greenLed.write(1); blueLed.write(1);
     }
-   }
\ No newline at end of file
+}