Merge pull request #1 from antrew/feature/control-robot-from-joystick

Feature/control robot from joystick

.gitignore

@@ -11,3 +11,6 @@ pre_processing_platform.txt
 
 # KDevelop
 *.kdev4
+
+# Arduino-Makefile
+build-*

common/common.h

@@ -17,19 +17,40 @@ uint8_t RADIO_ADDRESS[6] = "YABAB";
 
 // Set the PA Level low to prevent power supply related issues since this is a
 // getting_started sketch, and the likelihood of close proximity of the devices. RF24_PA_MAX is default.
-#define RADIO_POWER_LEVEL RF24_PA_MIN
+#define RADIO_POWER_LEVEL_ROBOT RF24_PA_MIN
+
+// use more power on the joystick
+#define RADIO_POWER_LEVEL_JOYSTICK RF24_PA_LOW
 
 // increase range by reducing the speed
 #define RADIO_DATA_RATE RF24_250KBPS
 
 // 108 - 2.508 Ghz - Above most Wifi Channels
 #define RADIO_CHANNEL 108
 
+enum commandType {
+	CONTROL,
+	TOGGLE_MOTORS,
+	CALIBRATE,
+	SET_PID_COEFFICIENTS,
+};
+
 struct radioMessage {
-	unsigned long timestamp;
-	unsigned long counter;
-	int8_t forward;
-	bool toggleMotors;
+	enum commandType command;
+	union {
+		struct {
+			int8_t forward;
+			int8_t rotate;
+		} control;
+		struct {
+			double pidP;
+			double pidI;
+			double pidD;
+			double speedP;
+			double rotateP;
+			double complementaryFilterT;
+		} pidCoefficients;
+	};
 };
 
 //Do not add code below this line

joystick/joystick.ino

@@ -9,6 +9,15 @@
 #define RADIO_PIN_CE 9
 #define RADIO_PIN_CS 10
 
+const double PID_P = 0.15;
+const double PID_I = 0.000001;
+const double PID_D = 0.002;
+
+const double SPEED_PID_P = 0.04;
+const double ROTATION_PID_P = 0.002;
+
+const double COMPLEMENTARY_FILTER_T = 1;
+
 RF24 radio(RADIO_PIN_CE, RADIO_PIN_CS);
 LiquidCrystal_I2C lcd(0x27, 16, 2);
 JoystickShield joystickShield;
@@ -17,6 +26,9 @@ boolean isInSettingsMode = false;
 
 const char * FW_ID = "joystick";
 
+char line[30];
+unsigned long counter = 0;
+
 void onSettingsButton() {
 	lcd.setCursor(0, 0);
 	if (isInSettingsMode) {
@@ -29,6 +41,25 @@ void onSettingsButton() {
 	isInSettingsMode = !isInSettingsMode;
 }
 
+void sendInitialMessage() {
+	struct radioMessage message;
+	message.command = SET_PID_COEFFICIENTS;
+	message.pidCoefficients.pidP = PID_P;
+	message.pidCoefficients.pidI = PID_I;
+	message.pidCoefficients.pidD = PID_D;
+
+	message.pidCoefficients.speedP = SPEED_PID_P;
+	message.pidCoefficients.rotateP = ROTATION_PID_P;
+
+	message.pidCoefficients.complementaryFilterT = COMPLEMENTARY_FILTER_T;
+
+	Serial.print("Sending initial message... ");
+	if (!radio.write(&message, sizeof(message))) {
+		Serial.println(F("failed"));
+	}
+	Serial.println(F("done"));
+}
+
 void setup() {
 	Serial.begin(115200);
 	printf_begin();
@@ -38,7 +69,7 @@ void setup() {
 
 	// Set the PA Level low to prevent power supply related issues since this is a
 	// getting_started sketch, and the likelihood of close proximity of the devices. RF24_PA_MAX is default.
-	radio.setPALevel(RADIO_POWER_LEVEL);
+	radio.setPALevel(RADIO_POWER_LEVEL_JOYSTICK);
 
 	// increase range by reducing the speed
 	radio.setDataRate(RADIO_DATA_RATE);
@@ -67,42 +98,49 @@ void setup() {
 	joystickShield.onLeftButton(&onSettingsButton);
 	joystickShield.onRightButton(&onSettingsButton);
 
-}
+	sendInitialMessage();
 
-char line[30];
-unsigned long counter = 0;
+}
 
 void loop() {
 
 	//joystickShield.processEvents();
+	// right is negative on the joystick
 	joystickShield.processCallbacks();
-	int x = joystickShield.xAmplitude();
-	int joystickY = joystickShield.yAmplitude();
+	int joystickX = - joystickShield.xAmplitude();
+	// forward is negative on the joystick
+	int joystickY = - joystickShield.yAmplitude();
 
 	struct radioMessage message;
-	message.timestamp = micros();
-	message.counter = counter++;
-	message.forward = joystickY;
 	if (joystickShield.isEButton()) {
 		Serial.println("E button pressed. Toggling motors. Waiting for the button to be released...");
-		message.toggleMotors = true;
+		message.command = TOGGLE_MOTORS;
 		while (joystickShield.isEButton()) {
 			// wait until the user releases the button
 			joystickShield.processEvents();
 		}
+	} else if (joystickShield.isFButton()) {
+		Serial.println("F button pressed. Calibrating. Waiting for the button to be released...");
+		message.command = CALIBRATE;
+		while (joystickShield.isFButton()) {
+			// wait until the user releases the button
+			joystickShield.processEvents();
+		}
 	} else {
-		message.toggleMotors = false;
+		message.command = CONTROL;
+		message.control.forward = joystickY;
+		message.control.rotate = joystickX;
 	}
 
 	Serial.print("Sending ");
-	Serial.print(message.timestamp);
+	Serial.print(message.command);
 	Serial.print("... ");
 	if (!radio.write(&message, sizeof(message))) {
 		Serial.println(F("failed"));
 	}
 	Serial.println(F("done"));
 
-	sprintf(line, "x=%4d ; y=%4d", x, joystickY);
+	sprintf(line, "x=%4d ; y=%4d", joystickX, joystickY);
 //	lcd.setCursor(0, 0);
 //	lcd.print(line);
 //

yababot/ComplementaryFilter.cpp

@@ -10,6 +10,7 @@
 #include <math.h>
 
 ComplementaryFilter::ComplementaryFilter() {
+	this->T = 1;
 }
 
 ComplementaryFilter::~ComplementaryFilter() {
@@ -30,20 +31,15 @@ void ComplementaryFilter::updateValue(
 
 	//complementary filter
 
-	// T defines the averaging period for accelerometerAngle
-	// bigger T increases the use of the gyroscope over the accelerometer
-	// smaller T increases the use of the accelerometer over the gyroscope
-	// use Arduino Serial Plotter over accelerometerAngle and angle to tune T
-	double T = 1; // in seconds
 	double K = T / (T + dt);
 	double gyroscopeAngle = gyroRate * dt;
 	angle = K * (angle + gyroscopeAngle) + (1 - K) * accelerometerAngle;
 
 	Serial.print(" accelerometerAngle ");
 	Serial.print(accelerometerAngle);
 
-//	Serial.print(" dt = ");
-//	Serial.print(dt);
+	Serial.print(" dt ");
+	Serial.print(dt);
 //	Serial.print(" K = ");
 //	Serial.print(K);
 
@@ -60,3 +56,7 @@ double ComplementaryFilter::getAngle() {
 double ComplementaryFilter::getDt() {
 	return this->dt;
 }
+
+void ComplementaryFilter::setT(double t) {
+	this->T = t;
+}

yababot/ComplementaryFilter.h

@@ -15,7 +15,14 @@ class ComplementaryFilter {
 	void updateValue(double accelerometerAngle, double gyroRate);
 	double getAngle();
 	double getDt();
+	void setT(double t);
 private:
+	// T defines the averaging period for accelerometerAngle
+	// bigger T increases the use of the gyroscope over the accelerometer
+	// smaller T increases the use of the accelerometer over the gyroscope
+	// use Arduino Serial Plotter over accelerometerAngle and angle to tune T
+	double T; // in seconds
+
 	unsigned long lastTime;
 	double angle;
 	double dt;

yababot/Makefile

@@ -7,5 +7,7 @@ USER_LIB_PATH=../lib
 
 ARDUINO_LIBS=Wire SPI Arduino-MPU6050 RF24
 
-BOARD_TAG    = uno
+#BOARD_TAG    = uno
+BOARD_TAG = pro328
+
 include /usr/share/arduino/Arduino.mk

yababot/PID.cpp

@@ -39,3 +39,9 @@ double PID::perform(double error, double dt) {
 
 	return u;
 }
+
+void PID::setCoefficients(double P, double I, double D) {
+	this->P = P;
+	this->I = I;
+	this->D = D;
+}

yababot/PID.h

@@ -5,6 +5,7 @@ class PID {
 public:
 	PID(double P, double I, double D);
 	double perform(double error, double dt);
+	void setCoefficients(double P, double I, double D);
 private:
 	double P;
 	double I;