Diff: main.cpp

Revision:

0:8f5db5085df7

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Sep 21 21:42:07 2020 +0000
@@ -0,0 +1,249 @@
+#include "FXAS21002.h"
+#include "FXOS8700Q.h"
+#include "mbed.h"
+#include "CarPWM.h"
+#include "receiver.h"
+#include "Motor.hpp"
+#include "rtos.h"
+
+#include "LED.hpp"
+#include "PIDController.hpp"
+
+#define PI 3.141593
+#define Ts 0.02			// Controller period(Seconds)
+#define END_THRESH 4	//For magnetometer calibration
+#define START_THRESH 10 //For magnetometer calibration
+#define MINIMUM_VELOCITY 15
+#define GYRO_PERIOD 15000 //us
+
+#define MIN -1.5
+#define MAX 1.5
+
+//Control Objetcs
+PwmOut servo(PTD1); // Servo connected to pin PTD3
+Motor *motor = new Motor(PTC3);
+FXOS8700Q_mag mag(PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1);
+FXOS8700Q_acc acc(PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1);
+FXAS21002 *gyro = new FXAS21002(PTE25, PTE24);
+//Leds Objects
+
+LED *led = new LED(PTD0, PTC4, PTC12, LED_RED, LED_GREEN, LED_BLUE);
+DigitalOut main_led(PTB2);
+//Protocol Objects
+Receiver rcv;
+EthernetInterface eth;
+
+// PID controller parameters and functions
+PIDController *pidController = new PIDController(motor, &servo, gyro);
+
+// Motor and servo variables
+float saved_velocity = 0;
+bool brake = false;
+
+// Magnetometer/Gyro variables and functions
+float max_x = 0, max_y = 0, min_x = 0, min_y = 0, x, y;
+MotionSensorDataUnits mag_data;
+MotionSensorDataCounts mag_raw;
+float processMagAngle();
+void magCal();
+
+// Protocol
+void readProtocol();
+
+int main()
+{
+	// Initializing sensors
+	main_led = 1;
+	acc.enable();
+	gyro->gyro_config(MODE_1);
+
+	// Set initial control configurations
+	motor->setVelocity(0);
+
+	// Protocol initialization
+
+	printf("Initializing Ethernet!\r\n");
+	led->set_leds_color_nxp(LED::RED);
+	led->set_leds_color_main(LED::RED);
+
+	eth.init(RECEIVER_IFACE_ADDR, RECEIVER_NETMASK_ADDR, RECEIVER_GATEWAY_ADDR);
+	eth.connect();
+	printf("Protocol initialized! \r\n");
+	gyro->start_measure(GYRO_PERIOD);
+	led->set_leds_color_nxp(LED::BLUE);
+	led->set_leds_color_main(LED::BLUE);
+	
+
+	rcv.set_socket();
+
+	main_led = 0;
+	
+	pidController->PIDAttach();
+
+	//main loop
+	while (1)
+	{
+		readProtocol();
+	}
+}
+
+void readProtocol()
+{
+	if (!rcv.receive())
+		return;
+	char msg = rcv.get_msg();
+
+	switch (msg)
+	{
+	case NONE:
+		break;
+	case BRAKE:
+		float intensity, b_wait;
+		rcv.get_brake(&intensity, &b_wait);
+		if (!brake)
+		{
+			led->set_leds_color_nxp(LED::YELLOW);
+			motor->stopJogging();
+			setServoPWM(0, servo);
+			pidController->PIDDetach();
+			motor->brakeMotor(intensity, b_wait);
+			pidController->PIDAttach();
+
+			saved_velocity = 0;
+			brake = true;
+		}
+		break;
+	case GYRO_ZERO:
+		gyro->stop_measure();
+		wait(0.05);
+		gyro->start_measure(GYRO_PERIOD);
+		break;
+	case ANG_SET:
+		led->set_leds_color_nxp(LED::PURPLE);
+		//printf("ANG_SET\r\n");
+		pidController->setGyroReference(gyro->get_angle());
+		pidController->initializeController();
+		break;
+	case ANG_RST:
+		//printf("ANG_RST\r\n");
+		pidController->setGyroReference(0);
+		led->set_leds_color_nxp(LED::PURPLE);
+		pidController->initializeController();
+		break;
+	case MAG_ANG_REF:
+		led->set_leds_color_nxp(LED::BLUE);
+		pidController->setTargetAngle(rcv.get_mag_ang_ref() - processMagAngle());
+		break;
+	case ABS_ANG_REF:
+		led->set_leds_color_nxp(LED::GREEN);
+		pidController->setTargetAngle(rcv.get_abs_ang_ref());
+		break;
+	case REL_ANG_REF:
+		led->set_leds_color_nxp(LED::RED);
+		pidController->setTargetAngle(rcv.get_rel_ang_ref() + gyro->get_angle() * PI / 180);
+		break;
+	case GND_VEL:
+		led->set_leds_color_nxp(LED::AQUA);
+		saved_velocity = rcv.get_gnd_vel();
+		if (saved_velocity > 0)
+		{
+			motor->setVelocity(saved_velocity);
+			if (abs(saved_velocity) > MINIMUM_VELOCITY)
+				brake = false;
+		}
+		break;
+	case JOG_VEL:
+		led->set_leds_color_nxp(LED::WHITE);
+		float p, r;
+		rcv.get_jog_vel(&p, &r);
+		if (p == 0 || r == 0)
+			motor->stopJogging();
+		else
+			motor->startJogging(r, p);
+		break;
+	case PID_PARAMS:
+		led->set_leds_color_nxp(LED::WHITE);
+		float arr[4];
+		rcv.get_pid_params(arr);
+		pidController->setPIDConstants(arr[0], arr[1], arr[2], arr[3]);
+
+		break;
+	case MAG_CALIB:
+		led->set_leds_color_nxp(LED::BLUE);
+		float mag[4];
+		rcv.get_mag_calib(mag);
+		max_x = mag[1];
+		max_y = mag[3];
+		min_x = mag[0];
+		min_y = mag[2];
+		//printf(" max_x = %f, max_y= %f, min_x= %f, min_y= %f\r\n",mag[1],mag[3],mag[0],mag[2]);
+		break;
+	case LED_ON:
+		led->set_leds_color_nxp(LED::WHITE);
+		led->set_leds_color_main(LED::WHITE);
+		main_led = 1;
+		break;
+	case LED_OFF:
+		led->turn_leds_off_nxp();
+		led->turn_leds_off_main();
+		main_led = 0;
+		break;
+	default:
+		//ser.printf("unknown command!\r\n");
+	}
+}
+
+/* Brake function, braking while the gyroscope is still integrating will cause considerably error in the measurement. */
+/* Function to normalize the magnetometer reading */
+void magCal()
+{
+	//red_led = 0;
+	printf("Starting Calibration");
+	mag.enable();
+	wait(0.01);
+	mag.getAxis(mag_data);
+	float x0 = max_x = min_y = mag_data.x;
+	float y0 = max_y = min_y = mag_data.y;
+	bool began = false;
+	while (!(began && abs(mag_data.x - x0) < END_THRESH && abs(mag_data.y - y0) < END_THRESH))
+	{
+		mag.getAxis(mag_data);
+		if (mag_data.x > max_x)
+			max_x = mag_data.x;
+		if (mag_data.y > max_y)
+			max_y = mag_data.y;
+		if (mag_data.y < min_y)
+			min_y = mag_data.y;
+		if (mag_data.x < min_x)
+			min_x = mag_data.x;
+		if (abs(mag_data.x - x0) > START_THRESH && abs(mag_data.y - y0) > START_THRESH)
+			began = true;
+		printf("began: %d  X-X0: %f , Y-Y0: %f \n\r", began, abs(mag_data.x - x0), abs(mag_data.y - y0));
+	}
+	printf("Calibration Completed: X_MAX = %f , Y_MAX = %f , X_MIN = %f and Y_MIN = %f \n\r", max_x, max_y, min_x, min_y);
+}
+
+/* Function to transform the magnetometer reading in angle(rad/s).*/
+float processMagAngle()
+{
+	// printf("starting ProcessMagAngle()\n\r");
+	float mag_lpf = 0;
+	Timer t1;
+	for (int i = 0; i < 20; i++)
+	{
+		//printf("\r\n");
+		//wait(0.1);
+		t1.start();
+		__disable_irq();
+		mag.getAxis(mag_data);
+		__enable_irq();
+		t1.stop();
+		x = 2 * (mag_data.x - min_x) / float(max_x - min_x) - 1;
+		y = 2 * (mag_data.y - min_y) / float(max_y - min_y) - 1;
+		mag_lpf = mag_lpf + (-atan2(y, x));
+		wait(0.015);
+	}
+	// wait(20*0.01);
+	// printf("Finished ProcessMagAngle() %d \n\r",t1.read_us());
+	return mag_lpf / 20;
+}