Skip to content
Snippets Groups Projects
Verified Commit 517aaf47 authored by Thomas Lambert's avatar Thomas Lambert :helicopter:
Browse files

feat(WIP): basic test for serial input

parent c89e05b5
No related branches found
No related tags found
No related merge requests found
/*******************************************************************************
Controller
Purpose: Controller for the MechaRaptor tandem flapping wing system.
Note: Currently, both ESC are fed the same signal, which means that the two
sets of wings flap at the same frequency. The phase between the two sets
will be adjusted manually by setting their initial positions as desired.
------------------------------------------------------------------------------
https://howtomechatronics.com/tutorials/arduino/arduino-brushless-motor-control-tutorial-esc-bldc/
------------------------------------------------------------------------------
(c) Copyright 2022 University of Liege
Author: Thomas Lambert <t.lambert@uliege.be>
ULiege - Aeroelasticity and Experimental Aerodynamics
MIT License
Repo: https://gitlab.uliege.be/thlamb/mecharaptor-controller
Controller
Purpose: Controller for the MechaRaptor tandem flapping wing system.
Note: Currently, both ESC are fed the same signal, which means that the two
sets of wings flap at the same frequency. The phase between the two sets
will be adjusted manually by setting their initial positions as desired.
--------------------------------------------------------------------------------
(c) Copyright 2022 University of Liege
Author: Thomas Lambert <t.lambert@uliege.be>
ULiege - Aeroelasticity and Experimental Aerodynamics
MIT License
Repo: https://gitlab.uliege.be/thlamb/mecharaptor-controller
*******************************************************************************/
/******************************************************************************
/*******************************************************************************
Includes
*******************************************************************************/
#include <Servo.h> // Control ESC by sending appropriate PWM signal
/******************************************************************************
/*******************************************************************************
Defines
*******************************************************************************/
#define SERVO_PIN_1 10 // ESC 1
......@@ -33,10 +31,10 @@
#define MAX_FREQ 10.0 // Maximum frequency
#define MIN_FREQ 1.0 // Minimum frequency
#define MAX_RPM 6000 // Motor maximum RPM (set in ESC)
#define MIN_RPM 1000 // Motor minimum RPM (set in ESC)
#define MAX_RPM 3000 // Motor maximum RPM (set in ESC)
#define MIN_RPM 750 // Motor minimum RPM (set in ESC)
/******************************************************************************
/*******************************************************************************
Globals
*******************************************************************************/
Servo ESC1;
......@@ -46,61 +44,81 @@ float freq = 0;
String mode;
/******************************************************************************
/*******************************************************************************
Setup function
Initializes everything and wait for serial input if switch in serial mode
inputs: N/A
outputs: N/A
******************************************************************************/
*******************************************************************************/
void setup() {
Serial.begin(9600); //115200 when we will use angle sensors
Serial.begin(9600); //115200 for proper angle sensor measurements
ESC1.attach(SERVO_PIN_1, 1000, 2000); // (pin, min pulse width, max pulse width)
ESC2.attach(SERVO_PIN_2, 1000, 2000); // (pin, min pulse width, max pulse width)
pinMode(SWITCH_PIN,INPUT);
Serial.print("[INFO]: Attaching ESCs ... ");
delay(5000); // Need delay otherwise motor start when serial begin
ESC1.attach(SERVO_PIN_1, MIN_SIG, MAX_SIG);
ESC2.attach(SERVO_PIN_2, MIN_SIG, MAX_SIG);
Serial.println("done!");
delay(1000); // Ensure proper initialization
pinMode(SWITCH_PIN, INPUT);
// Initialize modes
int switchState = digitalRead(SWITCH_PIN);
if(switchState == LOW){
mode = "MANUAL";
Serial.println("<MANUAL MODE>: Control motors using potentiometer");
Serial.println("[WARN]: Control motors using potentiometer");
} else{
// Must initialize at 0 otherwise won't start
ESC1.write(0);
ESC2.write(0);
mode = "SERIAL";
Serial.println("<SERIAL MODE>: Control motors using serial");
Serial.println("[WARN]: Control motors using serial");
freq = promptfreq();
}
Serial.println("[INFO]: Setup OK, starting ...");
delay(1000); // Ensure all is properly initialized
}
/******************************************************************************
/*******************************************************************************
loop function
purpose : implements the control and aquisition of the MicroRaptor
inputs: N/A
outputs: N/A
******************************************************************************/
*******************************************************************************/
void loop() {
int potAnal = analogRead(A0); // Always read potent (to log it for debug)
int escAng = 0; // Angle value to send to the ESC [0-180]
// Get proper value to send to ESC
/*if(mode =="SERIAL"){
* If nothing new, just continue
if(mode =="SERIAL"){
/* If nothing new, just continue
* If serial input, change value
* If serial is 0, stop
*/
// [TEST] Impose 40, then stops after 25s of runtime
if (millis() > 25000){
escAng = 0;
} else{
escAng = 40;
}
int escVal = freqtoesc(freq);
}
else{*/
else if(mode == "MANUAL"){
// Read and map potentiometer
int potAnal = analogRead(A0); // value between 0 and 1023
int escVal = map(potAnal, 0, 1023, 0, 180); // scale potentiometer value to use with servo lib
//}
escAng = map(potAnal, 0, 1023, 0, 180); // scale potentiometer value to use with servo lib
}
ESC1.write(escVal);
ESC2.write(escVal);
ESC1.write(escAng);
ESC2.write(escAng);
// [TEST] Write sine values for visualization in serial-studio
freq = 10;
......@@ -117,32 +135,31 @@ void loop() {
Serial.print(",");
Serial.print(potAnal*5.0/1023.0); // %3, Tension of potentiometer [V]
Serial.print(",");
Serial.print(escVal); // %4, Value sent to ESCs [-]
Serial.print(escAng); // %4, Angle value sent to ESCs [-]
Serial.print(",");
Serial.print(angleL1); // %5, Module 1 left wing angle [deg]
Serial.print(angleL1); // %5, Module 1 left wing angle [deg]
Serial.print(",");
Serial.print(angleR1); // %6, Module 1 right wing angle [deg]
Serial.print(angleR1); // %6, Module 1 right wing angle [deg]
Serial.print(",");
Serial.print(angleL2); // %7, Module 2 left wing angle [deg]
Serial.print(angleL2); // %7, Module 2 left wing angle [deg]
Serial.print(",");
Serial.print(angleR2); // %8, Module 2 right wing angle [deg]
Serial.print(angleR2); // %8, Module 2 right wing angle [deg]
Serial.println("*/"); // Frame finish sequence [*/]
//delay(5); // Reduce number of data to output
}
/******************************************************************************
/*******************************************************************************
promptfreq function
purpose : prompts the user for the frequency to use
inputs: N/A
outputs: freqency (float between 1 and 10)
******************************************************************************/
*******************************************************************************/
float promptfreq() {
float input;
Serial.print("Input a flapping frequency between ");
Serial.print("[INPUT]: Input a flapping frequency between ");
Serial.print(MIN_FREQ);
Serial.print(" and ");
Serial.print(MAX_FREQ);
......@@ -156,31 +173,30 @@ float promptfreq() {
input = Serial.parseFloat();
if (input > MAX_FREQ or input < MIN_FREQ) {
Serial.print("Frequency must be between ");
Serial.print("[ERROR]: Frequency must be between ");
Serial.print(MIN_FREQ);
Serial.print(" and ");
Serial.print(MAX_FREQ);
Serial.print(" [Hz]. You entered: ");
Serial.print(" [Hz]. Entered: ");
Serial.println(input);
}
}
Serial.print("Frequency correctly set to ");
Serial.print("[INFO]: Frequency correctly set to ");
Serial.print(input);
Serial.println(" Hz.");
Serial.println("<STARTING PROGRAM>.");
return input;
}
/******************************************************************************
/*******************************************************************************
freqtoesc function
purpose : converts frequency to ESC angle input
inputs: frequency
outputs: escVal
******************************************************************************/
*******************************************************************************/
int freqtoesc(float freq) {
int ret = 0;
......@@ -188,14 +204,14 @@ int freqtoesc(float freq) {
}
/******************************************************************************
/*******************************************************************************
stopmotor function
purpose : gently stops the motors
inputs: escVal
outputs: N/A
******************************************************************************/
*******************************************************************************/
void stopmotor(int curEscVal){
// Slow it down first
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment