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

feat(wip): start encoder support, major refactoring

parent 517aaf47
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
controller/controller.ino
+ 147
− 116
View file @ ebdce992
/*******************************************************************************
Controller
Purpose: Controller for the MechaRaptor tandem flapping wing system.
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.
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>
......@@ -21,162 +21,159 @@ Repo: https://gitlab.uliege.be/thlamb/mecharaptor-controller
#include <Servo.h> // Control ESC by sending appropriate PWM signal
/*******************************************************************************
Defines
Macros (use for pins only)
*******************************************************************************/
#define SERVO_PIN_1 10 // ESC 1
#define SERVO_PIN_2 9 // ESC 2
#define ESC1_PIN 10 // ESC Front motor
#define ESC2_PIN 9 // ESC Aft motor
#define SWITCH_PIN 7 // ON/OFF switch (mode control)
#define MAX_SIG 2000 // Max signal to the ESC [ms]
#define MIN_SIG 1000 // Min signal to the ESC [ms]
#define CLK_PIN 2 // Encoder CLK
#define DO_PIN 8 // Encoder Digital Output
#define MAX_FREQ 10.0 // Maximum frequency
#define MIN_FREQ 1.0 // Minimum frequency
#define MAX_RPM 3000 // Motor maximum RPM (set in ESC)
#define MIN_RPM 750 // Motor minimum RPM (set in ESC)
/*******************************************************************************
Globals
*******************************************************************************/
Servo ESC1;
Servo ESC2;
float freq = 0;
String mode;
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]
/*******************************************************************************
Setup function
const int gMAX_RPM = 3000; // Motor maximum RPM (set in ESC)
const int gMIN_RPM = 750; // Motor minimum RPM (set in ESC)
const int gGEAR_RATIO = 0.1; // Gear ratio between motor and wings
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
Initializes everything and wait for serial input if switch in serial mode
// [TODO]: Delete that
const int gMIN_FREQ = 3000; // Motor maximum RPM (set in ESC)
const int gMAX_FREQ = 3000; // Motor maximum RPM (set in ESC)
inputs: N/A
outputs: N/A
/*******************************************************************************
ARDUINO SETUP
*******************************************************************************/
void setup() {
Serial.begin(9600); //115200 for proper angle sensor measurements
Serial.begin(9600); //115200 for proper angle measurements, 9600 otherwise
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);
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);
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 switchState = digitalRead(SWITCH_PIN);
if(switchState == LOW){
mode = "MANUAL";
int switch_state = digitalRead(SWITCH_PIN);
if(switch_state == LOW){
gMode = "MANUAL";
Serial.println("[WARN]: Control motors using potentiometer");
} else{
// Must initialize at 0 otherwise won't start
ESC1.write(0);
ESC2.write(0);
gEsc1.write(0); // Must be initialized at 0 otherwise will not start
gEsc2.write(0);
mode = "SERIAL";
gMode = "SERIAL";
Serial.println("[WARN]: Control motors using serial");
freq = promptfreq();
}
Serial.println("[INFO]: Setup OK, starting ...");
gEsc_val = setupserial();
}
Serial.println("[INFO]: Setup OK, starting now...");
delay(1000); // Ensure all is properly initialized
}
/*******************************************************************************
loop function
purpose : implements the control and aquisition of the MicroRaptor
inputs: N/A
outputs: N/A
ARDUINO LOOP
Control the system
- In serial mode, we impose values based on serial inputs
- In manual mode, we use the potentiometer
Acquisition
- Get values from the 4 rotary encoders
*******************************************************************************/
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]
int potent = analogRead(A0); // Potentiometer value
unsigned int wing_angles[4]; // Wing angles (1L, 1R, 2L, 2R)
// Get proper value to send to ESC
if(mode =="SERIAL"){
if(gMode =="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;
if (Serial.available()){
int old = gEsc_val;
gEsc_val = Serial.parseFloat();
if (gEsc_val == 0){
stopmotor(old);
delay(2000);
exit(0);
}
}
//[FIXME] Get value properly
gEsc_val = 40;
}
else if(mode == "MANUAL"){
// Read and map potentiometer
escAng = map(potAnal, 0, 1023, 0, 180); // scale potentiometer value to use with servo lib
else if(gMode == "MANUAL"){
// Read and map potentiometer to use with ESC
gEsc_val = map(potent, 0, 1023, 0, 180);
}
ESC1.write(escAng);
ESC2.write(escAng);
gEsc1.write(gEsc_val);
gEsc2.write(gEsc_val);
// [TEST] Write sine values for visualization in serial-studio
freq = 10;
float angleL1 = sin(millis()*1000*freq*2*PI);
float angleR1 = cos(millis()*1000*freq*2*PI);
float angleL2 = -angleL1;
float angleR2 = -angleL2;
// [FIXME] Use readsensors
wing_angles[0] = (unsigned int)0;
wing_angles[1] = (unsigned int)1;
wing_angles[2] = (unsigned int)2;
wing_angles[3] = (unsigned int)3;
// Print all useful data to serial for processing in serial-studio
Serial.print("/*"); // Frame start sequence [/*]
Serial.print(mode); // %1, Mode
Serial.print(",");
Serial.print(millis()/1000.0); // %2, MCU runtime [s]
Serial.print(",");
Serial.print(potAnal*5.0/1023.0); // %3, Tension of potentiometer [V]
Serial.print(",");
Serial.print(escAng); // %4, Angle value sent to ESCs [-]
Serial.print(",");
Serial.print(angleL1); // %5, Module 1 left wing angle [deg]
Serial.print(",");
Serial.print(angleR1); // %6, Module 1 right wing angle [deg]
Serial.print(",");
Serial.print(angleL2); // %7, Module 2 left wing angle [deg]
Serial.print(",");
Serial.print(angleR2); // %8, Module 2 right wing angle [deg]
Serial.println("*/"); // Frame finish sequence [*/]
serialprint(gMode, potent, gEsc_val, wing_angles);
}
/*******************************************************************************
promptfreq function
OTHER FUNCTIONS
*******************************************************************************/
purpose : prompts the user for the frequency to use
inputs: N/A
outputs: freqency (float between 1 and 10)
*******************************************************************************/
float promptfreq() {
// Setup serial
// Get the initial value for the ESCs in serial mode
float setupserial() {
float input;
Serial.print("[INPUT]: Input a flapping frequency between ");
Serial.print(MIN_FREQ);
Serial.print(gMIN_FREQ);
Serial.print(" and ");
Serial.print(MAX_FREQ);
Serial.print(gMAX_FREQ);
Serial.println(" [Hz]");
while (input>MAX_FREQ or input<MIN_FREQ){
while (input>gMAX_FREQ or input<gMIN_FREQ){
while(Serial.available() == 0)
{
// Loop indefinitely while waiting for serial input
}
input = Serial.parseFloat();
if (input > MAX_FREQ or input < MIN_FREQ) {
if (input > gMAX_FREQ or input < gMIN_FREQ) {
Serial.print("[ERROR]: Frequency must be between ");
Serial.print(MIN_FREQ);
Serial.print(gMIN_FREQ);
Serial.print(" and ");
Serial.print(MAX_FREQ);
Serial.print(gMAX_FREQ);
Serial.print(" [Hz]. Entered: ");
Serial.println(input);
}
......@@ -189,39 +186,73 @@ float promptfreq() {
}
/*******************************************************************************
freqtoesc function
purpose : converts frequency to ESC angle input
inputs: frequency
outputs: escVal
*******************************************************************************/
// Converts frequence to ESC angle to use with Servo library
int freqtoesc(float freq) {
int ret = 0;
// Use mapping between freq and 0,180
// Should normally require calibration though...
return ret;
}
/*******************************************************************************
stopmotor function
// Gracefully stops the motor in serial mode
void stopmotor(int old_esc_val){
purpose : gently stops the motors
Serial.println("[INFO] MOTOR STOP INITIATED");
inputs: escVal
outputs: N/A
*******************************************************************************/
void stopmotor(int curEscVal){
if (old_esc_val > 40){
Serial.println("[INFO] Slowing down motor for 5 sec...");
gEsc1.write(40);
gEsc2.write(40);
delay(5000);
}
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);
}
// Slow it down first
if (curEscVal > 40){
ESC1.write(40);
ESC2.write(40);
delay(2000);
//[TODO] FIX THAT
unsigned int readSensor(uint8_t csn){
unsigned int dataOut = 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
dataOut = (dataOut << 1) | digitalRead(DO_PIN); //shift all the entering data to the left and past the pin state to it. 1e bit is MSB
}
// Shut it off
ESC1.write(0);
ESC2.write(0);
digitalWrite(csn, HIGH); //deselects the encoder from reading
return dataOut;
}
// Output data to use with serial-studio
void serialprint(String mode, int potent, int esc_val, unsigned int wing_angles[]) {
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);
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