/*
--------------------------------------------
-------- XJME1010 Coursework 2023 --------
-------- Name: --------
-------- Username: --------
--------------------------------------------
*/
#include <PID_v1.h>
#include <Servo.h>
#define start_Up_Led_Pin 6 //Start-up LED = Pin 6
#define scanning_Led_Pin 5 //Scanning LED = Pin 5
#define scan_End_Led_Pin 2 //Scan End LED = Pin 4
#define Motor_Control_Pin 3 //Motor Controller Control = Pin 3
#define potentiometer_Pin A5 //The output Vpot can be measured using Arduino Pin A5
Servo servoMotor;
double Kp = 18, Ki = 0, Kd = 0;
double Input, Output, Setpoint = 0.00; //The aim is to develop a balance control system which moves your drone to a stable horizontal position
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);
uint8_t system_State = 0x00; //The process is defined by a series of numbered stages.
unsigned long system_Time; //Start timing system time after power on
unsigned long reach_Time; //Scan Timer
unsigned long Scan_Lsat_Timer; //your Scan Timer reaches 5 seconds
float controller_Time = 0.00; //Start timing control time after entering control
void setup() {
//As your control program starts it must initialise the system.
Serial.begin(9600); //Setup your Serial communication so you can send messages.
Serial.println("0.System Started"); //0.System Started
//Configure your hardware connections to the sensor and motors.
pinMode(start_Up_Led_Pin, OUTPUT); //Set Start-up LED as OUTPUT
digitalWrite(start_Up_Led_Pin, LOW); //Turn off Start-up LED
pinMode(scanning_Led_Pin, OUTPUT); //Set Scanning LED as OUTPUT
digitalWrite(scanning_Led_Pin, LOW); //Turn off Scanning LED
pinMode(scan_End_Led_Pin, OUTPUT); //Set Scan End LED as OUTPUT
digitalWrite(scan_End_Led_Pin, LOW); //Turn off Scan End LED
servoMotor.attach(Motor_Control_Pin); //Attach motor to 3
myPID.SetMode(AUTOMATIC);
myPID.SetOutputLimits(-40, 40); // Set PID output range
//Initialise the motors by sending a 0 (zero) signal to motors.
servoMotor.write(0); //sending a 0 (zero) signal to the motor
Serial.println("1.System Initiated"); //1.System Initiated
}
void loop() {
switch (system_State) { //Judge the current system stage
case 0x00:
{
system_Time = millis(); //Get system time
digitalWrite(start_Up_Led_Pin, HIGH); //Signal that your balance controller has started by lighting the Start-up LED for 1 second
Serial.println("2.Controller Starting"); //2.Controller Starting
system_State++; //system stage +1
};
break;
case 0x01:
{
if (millis() - system_Time >= 1000) { //1 second
digitalWrite(start_Up_Led_Pin, LOW); //Turn off Start-up LED
Serial.println("Time,\tAngle,\tError,\tControl Signal,\tMotor "); //Time,Angle,Error,Control Signal,Motor L, Motor R
system_Time = millis(); //Get system time
reach_Time = 0; //Update Scan Timer
system_State++; //system stage +1
}
};
break;
case 0x02:
{
if (millis() - system_Time >= 40) { //Your controller should now start. It should run continuously at 25Hz.
system_Time = millis(); //Get system time
controller_Time += 40 / 1000.0; //25Hz
int Vpot = constrain(analogRead(potentiometer_Pin), 329, 701); //a. The controller first reads the angle sensor to determine the current angular position of the drone
if (Vpot <= 536) {
Input = map(Vpot, 329, 536, -40, 0);
} else {
Input = map(Vpot, 536, 701, 0, 40);
}
//b. Now your controller should calculate a motor control signal
myPID.Compute(); //PID operation
float Error = Setpoint - Output;
float Motor = Output + 90;
//c. Convert the motor control signal into control signals for the motor
servoMotor.write(Motor);
//d. Send telemetry information using Serial communications
Serial.print(controller_Time); //Time
Serial.print(",");
Serial.print(Input); //Angle
Serial.print(",");
Serial.print(Error); //Error
Serial.print(",");
Serial.print(Output); //Control Signal
Serial.print(",");
Serial.println(Motor/180*100); //Motor
//e. Check if you moved within 5 degrees of the target angle (e.g. horizontal)
if (abs(Error) < 5) { //i. Yes? Start your 'Scan Timer' and Light the Scanning LED
digitalWrite(scanning_Led_Pin, HIGH);
if (millis() - reach_Time > 5000) { //f. Continue the controller until your Scan Timer reaches 5 seconds
digitalWrite(scanning_Led_Pin, LOW); //Turn off Scanning LED
digitalWrite(scan_End_Led_Pin, HIGH); //Shutdown: Light the Shutdown LED for 1 second and turn-off the motors
system_Time = millis(); //Get system time
system_State++; //system stage +1
}
} else {
digitalWrite(scanning_Led_Pin, LOW); //Turn off Scanning LED
reach_Time = millis();
}
}
};
break;
case 0x03:
{
if (millis() - system_Time >= 1000) {
digitalWrite(scan_End_Led_Pin, LOW); //Shutdown: Light the Shutdown LED for 1 second and turn-off the motors
servoMotor.write(0); //turn-off the motor
Serial.println("4. Shutdown"); //4. Shutdown
system_State++; //system stage +1
}
};
break;
default: break;
}
}