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

feat(WIP): ground work for serial ctrl

parent 69913cb6
No related branches found
No related tags found
No related merge requests found
......@@ -30,55 +30,136 @@ byte switchPin = 7; // ON/OFF switch (safety feature)
Servo ESC1;
Servo ESC2;
// Attach ESC to their pins
// Declare constants
const float maxFreq = 10.0; // Maximum frequency
const float minFreq = 1.0; // Minimum frequency
const int maxRPM = 6000; // Motor maximum RPM (set in ESC)
const int minRPM = 1000; // Motor minimum RPM (set in ESC)
// Declare global variables
float freq = 0;
String mode;
void setup() {
// Initialize components
Serial.begin(9600);
ESC1.attach(servoPin1);
ESC2.attach(servoPin2);
ESC1.attach(servoPin1, 1000, 2000); // (pin, min pulse width, max pulse width)
ESC2.attach(servoPin2, 1000, 2000); // (pin, min pulse width, max pulse width)
pinMode(switchPin,INPUT);
delay(1000); // Ensure proper initialization
// Set type of operation and related parameters if applicable
int switchState = digitalRead(switchPin);
if(switchState == LOW){
mode = "MANUAL";
Serial.println("<MANUAL MODE>: Control motors using potentiometer");
} else{
mode = "SERIAL";
Serial.println("<SERIAL MODE>: Control motors using serial");
freq = promptfreq();
}
}
void loop() {
// Read ON/OFF switch state
int switchState = digitalRead(switchPin);
String armed;
// Read and map potentiometer
int potAnal = analogRead(A0); // value between 0 and 1023
int potVal = map(potAnal, 0, 1023, 0, 180); // scale potentiometer value to use with servo lib
// [TEST] Write sine values for visualization in serial-studio
float freq = 10;
float angleL = sin(millis()*1000*freq);
float angleR = cos(millis()*1000*freq);
// Send values to ESCs (currently both receive the same input)
if (switchState == LOW){
armed = "OFF";
ESC1.write(0);
ESC2.write(0);
} else {
armed = "ON";
ESC1.write(potVal);
ESC2.write(potVal);
// Get proper value to send to ESC
/*if(mode =="SERIAL"){
* If nothing new, just continue
* If serial input, change value
* If serial is 0, stop
int escVal = freqtoesc(freq);
}
else{*/
// 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
//}
ESC1.write(escVal);
ESC2.write(escVal);
// [TEST] Write sine values for visualization in serial-studio
freq = 10;
float angleL = sin(millis()*1000*freq*2*PI);
float angleR = cos(millis()*1000*freq*2*PI);
// Print all useful data to serial for processing in serial-studio
Serial.print("/*"); // Frame start sequence [/*]
Serial.print(millis()/1000.0); // %1, MCU runtime [s]
Serial.print(mode); // %1, Mode
Serial.print(",");
Serial.print(potAnal*5.0/1023.0); // %2, Tension of potentiometer [V]
Serial.print(millis()/1000.0); // %2, MCU runtime [s]
Serial.print(",");
Serial.print(potVal); // %3, Mapped value of potentiometer [-]
Serial.print(potAnal*5.0/1023.0); // %3, Tension of potentiometer [V]
Serial.print(",");
Serial.print(angleL); // %4, Left wing angle [deg]
Serial.print(escVal); // %4, Value sent to ESCs [-]
Serial.print(",");
Serial.print(angleR); // %5, Right wing angle [deg]
Serial.print(angleL); // %5, Left wing angle [deg]
Serial.print(",");
Serial.print(armed); // %6, Armed status
Serial.print(angleR); // %6, Right wing angle [deg]
Serial.println("*/"); // Frame finish sequence [*/]
delay(50); // Reduce number of data to output
//delay(1); // Reduce number of data to output
}
// Prompt user to input the frequency
float promptfreq() {
float input;
Serial.print("Input a flapping frequency between ");
Serial.print(minFreq);
Serial.print(" and ");
Serial.print(maxFreq);
Serial.println(" [Hz]");
while (input>maxFreq or input<minFreq){
while(Serial.available() == 0)
{
// Loop indefinitely while waiting for serial input
}
input = Serial.parseFloat();
if (input > maxFreq or input < minFreq) {
Serial.print("Frequency must be between ");
Serial.print(minFreq);
Serial.print(" and ");
Serial.print(maxFreq);
Serial.print(" [Hz]. You entered: ");
Serial.println(input);
}
}
Serial.print("Frequency correctly set to ");
Serial.print(input);
Serial.println(" Hz.");
Serial.println("<STARTING PROGRAM>.");
return input;
}
// Convert frequency to ESC input
int freqtoesc(float freq) {
int ret = 0;
return ret;
}
// Stop the motor properly
void stopmotor(int curEscVal){
// Slow it down first
if (curEscVal > 40){
ESC1.write(40);
ESC2.write(40);
delay(2000);
}
// Shut it off
ESC1.write(0);
ESC2.write(0);
}
......@@ -13,8 +13,8 @@
"log": false,
"max": 0,
"min": 0,
"title": "Runtime",
"units": "s",
"title": "Mode",
"units": "",
"value": "%1",
"widget": ""
},
......@@ -27,9 +27,9 @@
"log": false,
"max": 0,
"min": 0,
"title": "Ready",
"units": "",
"value": "%6",
"title": "Runtime",
"units": "s",
"value": "%2",
"widget": ""
}
],
......@@ -49,7 +49,7 @@
"min": 0,
"title": "Pot. tension",
"units": "V",
"value": "%2",
"value": "%3",
"widget": ""
},
{
......@@ -61,9 +61,9 @@
"log": false,
"max": 180,
"min": 0,
"title": "Pot. val",
"title": "ESC val",
"units": "",
"value": "%3",
"value": "%4",
"widget": "gauge"
}
],
......@@ -83,7 +83,7 @@
"min": 0,
"title": "Left",
"units": "deg",
"value": "%4",
"value": "%5",
"widget": ""
},
{
......@@ -97,7 +97,7 @@
"min": 0,
"title": "Right",
"units": "deg",
"value": "%5",
"value": "%6",
"widget": ""
}
],
......
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