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

style: astyle formatter

parent 2aeeb284
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
controller/controller.ino
+ 151
− 152
View file @ 6beeca72
......@@ -18,26 +18,26 @@ Repo: https://gitlab.uliege.be/thlamb/mecharaptor-controller
/*******************************************************************************
Includes
*******************************************************************************/
#include <Servo.h> // Control ESC by sending appropriate PWM signal
#include <Servo.h> // Control ESC by sending appropriate PWM signal
/*******************************************************************************
Macros (use for pins only)
*******************************************************************************/
#define ESC1_PIN 10 // ESC Front motor
#define ESC2_PIN 9 // ESC Aft motor
#define SWITCH_PIN 7 // ON/OFF switch (mode control)
#define CLK_PIN 2 // Encoder CLK
#define DO_PIN 8 // Encoder Digital Output
#define ESC1_PIN 10 // ESC Front motor
#define ESC2_PIN 9 // ESC Aft motor
#define SWITCH_PIN 7 // ON/OFF switch (mode control)
#define CLK_PIN 2 // Encoder CLK
#define DO_PIN 8 // Encoder Digital Output
/*******************************************************************************
Globals
*******************************************************************************/
const uint8_t gCSN_PINS[] = {3,4,5,6}; // Encoder Chip Select pins
const uint8_t gCSN_PINS[] = { 3, 4, 5, 6 }; // Encoder Chip Select pins
const int gMAX_SIG = 2000; // Max signal to the ESC [ms]
const int gMIN_SIG = 1000; // Min signal to the ESC [ms]
const int gMAX_SIG = 2000; // Max signal to the ESC [ms]
const int gMIN_SIG = 1000; // Min signal to the ESC [ms]
const int gMAX_RPM = 3000; // Motor maximum RPM (set in ESC)
const int gMIN_RPM = 750; // Motor minimum RPM (set in ESC)
......@@ -45,8 +45,8 @@ const float gGEAR_RATIO = 0.1; // Gear ratio between motor and wings
const int gMAX_ESC_VAL = 160; // Value at which motor is at max RPM
const int gMIN_ESC_VAL = 40; // Min value for ESC to start motor (trial-error)
Servo gEsc1; // ESC for front motor
Servo gEsc2; // ESC for aft motor
Servo gEsc1; // ESC for front motor
Servo gEsc2; // ESC for aft motor
int gEsc_val = 0; // ESC angle (to use with the Servo library) [0-180]
String gMode; // Mode of operation
......@@ -56,44 +56,44 @@ String gMode; // Mode of operation
ARDUINO SETUP
*******************************************************************************/
void setup() {
//Serial.begin(9600); //[DEBUG]: For debugging only
Serial.begin(115200); // Need for good precision
Serial.print("[INFO]: Attaching ESCs ... ");
delay(5000); // Prevents motor from starting automatically with serial
gEsc1.attach(ESC1_PIN, gMIN_SIG, gMAX_SIG);
gEsc2.attach(ESC2_PIN, gMIN_SIG, gMAX_SIG);
Serial.println("done!");
pinMode(SWITCH_PIN, INPUT);
// [DEBUG] Comment all that as long as PCB not properly soldered
//for (int i = 0; i < sizeof(gCSN_PINS); i++) {
// pinMode(gCSN_PINS[i], OUTPUT);
// digitalWrite(gCSN_PINS[i], HIGH);
//}
//pinMode(CLK_PIN, OUTPUT);
//pinMode(DO_PIN, INPUT);
//digitalWrite(CLK_PIN, HIGH);
// Initialize modes
int switch_state = digitalRead(SWITCH_PIN);
if(switch_state == LOW){
gMode = "MANUAL";
Serial.println("[WARN]: Control motors using potentiometer");
} else{
gEsc1.write(0); // Must be initialized at 0 otherwise will not start
gEsc2.write(0);
//Serial.begin(9600); //[DEBUG]: For debugging only
Serial.begin(115200); // Need for good precision
Serial.print("[INFO]: Attaching ESCs ... ");
delay(5000); // Prevents motor from starting automatically with serial
gEsc1.attach(ESC1_PIN, gMIN_SIG, gMAX_SIG);
gEsc2.attach(ESC2_PIN, gMIN_SIG, gMAX_SIG);
Serial.println("done!");
pinMode(SWITCH_PIN, INPUT);
// [DEBUG] Comment all that as long as PCB not properly soldered
//for (int i = 0; i < sizeof(gCSN_PINS); i++) {
// pinMode(gCSN_PINS[i], OUTPUT);
// digitalWrite(gCSN_PINS[i], HIGH);
//}
//pinMode(CLK_PIN, OUTPUT);
//pinMode(DO_PIN, INPUT);
//digitalWrite(CLK_PIN, HIGH);
// Initialize modes
int switch_state = digitalRead(SWITCH_PIN);
if (switch_state == LOW) {
gMode = "MANUAL";
Serial.println("[WARN]: Control motors using potentiometer");
gMode = "SERIAL";
Serial.println("[WARN]: Control motors using serial");
} else {
gEsc1.write(0); // Must be initialized at 0 otherwise will not start
gEsc2.write(0);
gMode = "SERIAL";
Serial.println("[WARN]: Control motors using serial");
gEsc_val = SetupSerial();
}
Serial.println("[INFO]: Setup OK, starting now...");
delay(1000); // Ensure all is properly initialized
gEsc_val = SetupSerial();
}
Serial.println("[INFO]: Setup OK, starting now...");
delay(1000); // Ensure all is properly initialized
}
......@@ -107,33 +107,32 @@ void setup() {
*******************************************************************************/
void loop() {
int potent = analogRead(A0); // Potentiometer value
unsigned int wing_angles[4]; // Wing angles (1L, 1R, 2L, 2R)
int potent = analogRead(A0); // Potentiometer value
unsigned int wing_angles[4]; // Wing angles (1L, 1R, 2L, 2R)
if(gMode =="SERIAL" and Serial.available()){
if (gMode == "SERIAL" and Serial.available()) {
float input = Serial.parseFloat();
if (input == 0){
StopMotor(gEsc_val);
} else {
gEsc_val = FreqToEsc(input);
}
float input = Serial.parseFloat();
if (input == 0) {
StopMotor(gEsc_val);
} else {
gEsc_val = FreqToEsc(input);
}
}
else if(gMode == "MANUAL"){
gEsc_val = map(potent, 0, 1023, 0, 180);
}
} else if (gMode == "MANUAL") {
gEsc_val = map(potent, 0, 1023, 0, 180);
}
// Write value to ESCs
gEsc1.write(gEsc_val);
gEsc2.write(gEsc_val);
// Write value to ESCs
gEsc1.write(gEsc_val);
gEsc2.write(gEsc_val);
// Read value from rotary encoders
for (int i = 0; i < sizeof(gCSN_PINS); i++){
wing_angles[i] = ReadSensor(gCSN_PINS[i]);
}
// Read value from rotary encoders
for (int i = 0; i < sizeof(gCSN_PINS); i++) {
wing_angles[i] = ReadSensor(gCSN_PINS[i]);
}
SerialPrint(gMode, potent, gEsc_val, wing_angles);
SerialPrint(gMode, potent, gEsc_val, wing_angles);
}
......@@ -144,120 +143,120 @@ void loop() {
// Setup serial
// Get the initial value for the ESCs in serial mode
float SetupSerial() {
float input;
float min_freq = RpmToFreq(gMIN_RPM);
float max_freq = RpmToFreq(gMAX_RPM);
Serial.print("[INPUT]: Input a flapping frequency between ");
Serial.print(min_freq);
Serial.print(" and ");
Serial.print(max_freq);
Serial.println(" [Hz]");
while (input>max_freq or input<min_freq){
// Loop indefinitely while waiting for serial input
while(Serial.available() == 0) { }
input = Serial.parseFloat();
if (input > max_freq or input < min_freq) {
Serial.print("[ERROR]: Frequency must be between ");
Serial.print(min_freq);
Serial.print(" and ");
Serial.print(max_freq);
Serial.print(" [Hz]. Entered: ");
Serial.println(input);
float input;
float min_freq = RpmToFreq(gMIN_RPM);
float max_freq = RpmToFreq(gMAX_RPM);
Serial.print("[INPUT]: Input a flapping frequency between ");
Serial.print(min_freq);
Serial.print(" and ");
Serial.print(max_freq);
Serial.println(" [Hz]");
while (input > max_freq or input < min_freq) {
// Loop indefinitely while waiting for serial input
while (Serial.available() == 0) {}
input = Serial.parseFloat();
if (input > max_freq or input < min_freq) {
Serial.print("[ERROR]: Frequency must be between ");
Serial.print(min_freq);
Serial.print(" and ");
Serial.print(max_freq);
Serial.print(" [Hz]. Entered: ");
Serial.println(input);
}
}
}
Serial.print("[INFO]: Frequency correctly set to ");
Serial.print(input);
Serial.println(" Hz.");
Serial.print("[INFO]: Frequency correctly set to ");
Serial.print(input);
Serial.println(" Hz.");
return FreqToEsc(input);
return FreqToEsc(input);
}
// Converts frequence to ESC angle to use with Servo library
int FreqToEsc(float freq) {
// Need to scale times 100 so we can have better precision
int esc_val = map(freq*100, 0, 100*RpmToFreq(gMAX_RPM), 0, 180);
// Need to scale times 100 so we can have better precision
int esc_val = map(freq * 100, 0, 100 * RpmToFreq(gMAX_RPM), 0, 180);
if (esc_val < gMIN_ESC_VAL) {
esc_val = gMIN_ESC_VAL;
} else if (esc_val > gMAX_ESC_VAL) {
esc_val = gMAX_ESC_VAL;
}
if (esc_val < gMIN_ESC_VAL) {
esc_val = gMIN_ESC_VAL;
} else if (esc_val > gMAX_ESC_VAL) {
esc_val = gMAX_ESC_VAL;
}
return esc_val;
return esc_val;
}
// Converts RPM to Flapping frequency
float RpmToFreq(int rpm) {
return rpm*gGEAR_RATIO/60.0;
return rpm * gGEAR_RATIO / 60.0;
}
// Gracefully stops the motor in serial mode
void StopMotor(int old_esc_val){
void StopMotor(int old_esc_val) {
Serial.println("[INFO] MOTOR STOP INITIATED");
Serial.println("[INFO] MOTOR STOP INITIATED");
if (old_esc_val > gMIN_ESC_VAL){
Serial.println("[INFO] Slowing down motor for 5 sec...");
gEsc1.write(gMIN_ESC_VAL);
gEsc2.write(gMIN_ESC_VAL);
delay(5000);
}
if (old_esc_val > gMIN_ESC_VAL) {
Serial.println("[INFO] Slowing down motor for 5 sec...");
gEsc1.write(gMIN_ESC_VAL);
gEsc2.write(gMIN_ESC_VAL);
delay(5000);
}
gEsc1.write(0);
gEsc2.write(0);
Serial.println("[INFO] Motor stopped completely");
gEsc1.write(0);
gEsc2.write(0);
Serial.println("[INFO] Motor stopped completely");
// Lock system in infinite loop to prevent any restart
delay(1000);
while(1);
// Lock system in infinite loop to prevent any restart
delay(1000);
while (1) {}
}
//Read 12-bit rotary encoder (RobinL modified by linarism on forum.arduino.cc)
unsigned int ReadSensor(uint8_t csn){
unsigned int ret = 0;
digitalWrite(csn, LOW);
delayMicroseconds(1); //Waiting for Tclkfe
//Passing 12 times, from 0 to 11
for(int x=0; x<12; x++){
digitalWrite(CLK_PIN, LOW);
delayMicroseconds(1); //Tclk/2
digitalWrite(CLK_PIN, HIGH);
delayMicroseconds(1); //Tdo valid, like Tclk/2
ret = (ret << 1) | digitalRead(DO_PIN); //shift all the entering data to the left and past the pin state to it. 1e bit is MSB
}
digitalWrite(csn, HIGH); //deselects the encoder from reading
return ret;
unsigned int ReadSensor(uint8_t csn) {
unsigned int ret = 0;
digitalWrite(csn, LOW);
delayMicroseconds(1); //Waiting for Tclkfe
//Passing 12 times, from 0 to 11
for (int x = 0; x < 12; x++) {
digitalWrite(CLK_PIN, LOW);
delayMicroseconds(1); //Tclk/2
digitalWrite(CLK_PIN, HIGH);
delayMicroseconds(1); //Tdo valid, like Tclk/2
ret = (ret << 1) | digitalRead(DO_PIN); //shift all the entering data to the left and past the pin state to it. 1e bit is MSB
}
digitalWrite(csn, HIGH); //deselects the encoder from reading
return ret;
}
// Output data to use with serial-studio
void SerialPrint(String mode, int potent, int esc_val, unsigned int wing_angles[]) {
float freq = map(esc_val, 0, 180, 0, RpmToFreq(gMAX_RPM)*100.0)/100.0;
Serial.print("/*");
Serial.print(mode);
Serial.print(",");
Serial.print(millis()/1000.0);
Serial.print(",");
Serial.print(potent*5.0/1023.0); // Converts pot val into proper tension
Serial.print(",");
Serial.print(esc_val);
Serial.print(",");
Serial.print(freq); // [FIXME] Is it useful?
for (int i = 0; i < sizeof(gCSN_PINS); i++){
float freq = map(esc_val, 0, 180, 0, RpmToFreq(gMAX_RPM) * 100.0) / 100.0;
Serial.print("/*");
Serial.print(mode);
Serial.print(",");
Serial.print(millis() / 1000.0);
Serial.print(",");
Serial.print(potent * 5.0 / 1023.0); // Converts pot val into proper tension
Serial.print(",");
Serial.print(wing_angles[i]);
}
Serial.println("*/");
Serial.print(esc_val);
Serial.print(",");
Serial.print(freq); // [FIXME] Is it useful?
for (int i = 0; i < sizeof(gCSN_PINS); i++) {
Serial.print(",");
Serial.print(wing_angles[i]);
}
Serial.println("*/");
}
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment