From 6edd58f3ba09611886bbabbee57345e4889bb18d Mon Sep 17 00:00:00 2001
From: Thomas Lambert <t.lambert@uliege.be>
Date: Mon, 21 Nov 2022 13:05:08 +0100
Subject: [PATCH] feat: PSU sensor and angle calibration
---
controller/controller.ino | 41 +++++++++++++++++++++++++++------------
1 file changed, 29 insertions(+), 12 deletions(-)
diff --git a/controller/controller.ino b/controller/controller.ino
index ef4001c..7e362e9 100644
--- a/controller/controller.ino
+++ b/controller/controller.ino
@@ -35,22 +35,28 @@ Repo: https://gitlab.uliege.be/thlamb/mecharaptor-controller
#define CSN_PINS \
{ 6, 5, 4, 3 } // Encoder Chip Select pins
-#define CUR1_PIN A1 // Current measurement for Front motor
+#define CUR1_PIN A4 // Current measurement for Front motor
#define CUR2_PIN A2 // Current measurement for Aft motor
#define TENS1_PIN A3 // Tension measurement for Front motor
-#define TENS2_PIN A4 // Tension measurement for Aft motor
+#define TENS2_PIN A1 // Tension measurement for Aft motor
// Values
-#define DIV_RES1 9000.0 // Resistance of R1 in the voltage dividers
-#define DIV_RES2 1000.0 // Resistance of R2 in the voltage dividers
+#define DIV_RES1 47800.0 // Resistance of R1 in the voltage dividers
+#define DIV_RES2 6800.0 // Resistance of R2 in the voltage dividers
#define SEN0098_VCC 5 // Supply voltage of the current sensors
#define SEN0098_SENSI 20.0 // [FIXME] VERIFY!
#define SEN0098_VIOUT 0.12 // [FIXME] VERIFY!
#define SEN0098_OFFSET 0.007 // [FIXME] VERIFY!
+
#define GEAR_RATIO 0.1 // Gear ratio between motor and wings
+// Calibration of encoders (NEEDS TO BE REDONE WHEN ENCODERS ARE MOVED)
+#define ENC_PRECISION 4096 // 12-Bits encoders (nb of points per rot)
+#define ENC_ORIENT {1, -1, 1, -1} // Encoder orientation
+#define MAX_WING_POS {222.1, 117.77, 117, 117} // Maximum wing angle (returned by encoder value)
+#define TRUE_MAX_ANGLE 37.9 // Maximum wing angle (measured on setup)
/*******************************************************************************
Globals (for things that I can change manually)
@@ -66,6 +72,8 @@ const uint8_t gMAX_ESC_VAL = 160; // Value at which motor is at max RPM
const uint8_t gMIN_ESC_VAL = 40; // Min value for ESC to start motor (trial-error)
const uint8_t gCSN_PINS[] = CSN_PINS; // Encoder Chip Select pins
+const int8_t gENC_ORIENT[] = ENC_ORIENT; // Encoder orientation
+const uint16_t gMAX_WING_POS[] = MAX_WING_POS; // Encoder maximum recorder position
const float gCUR_FACT = SEN0098_SENSI / 1000.0; // Factor for current measurement
const float gCUR_QOV = SEN0098_VIOUT * SEN0098_VCC; // [FIXME] Verify
@@ -75,6 +83,7 @@ Servo gEsc2; // ESC for aft motor
uint8_t gEsc_val = 0; // ESC angle (to use with the Servo library) [0-180]
String gMode; // Mode of operation
+uint16_t max_wing_pos[4]; // Encoder maximum recorder position
/*******************************************************************************
@@ -134,7 +143,7 @@ void setup() {
void loop() {
int potent = analogRead(POT_PIN); // Potentiometer value
- uint16_t wing_angles[sizeof(gCSN_PINS)]; // Wing angles (1L, 1R, 2L, 2R)
+ unsigned int wing_angles[sizeof(gCSN_PINS)]; // Wing angles (1L, 1R, 2L, 2R)
if (gMode == "SERIAL" and Serial.available()) {
@@ -155,7 +164,7 @@ void loop() {
// Read value from rotary encoders
for (int i = 0; i < sizeof(gCSN_PINS); i++) {
- wing_angles[i] = ReadSensor(gCSN_PINS[i]);
+ wing_angles[i] = ReadSensor(gCSN_PINS[i], gENC_ORIENT[i]);
}
// Read tension and current from PSU
@@ -251,8 +260,8 @@ void StopMotor(int old_esc_val) {
//Read 12-bit rotary encoder (RobinL modified by linarism on forum.arduino.cc)
-uint16_t ReadSensor(uint8_t csn) {
- uint16_t ret = 0;
+unsigned int ReadSensor(uint8_t csn, int8_t orient) {
+ unsigned int ret = 0;
digitalWrite(csn, LOW);
delayMicroseconds(1); //Waiting for Tclkfe
@@ -267,12 +276,12 @@ uint16_t ReadSensor(uint8_t csn) {
}
digitalWrite(csn, HIGH); //deselects the encoder from reading
- return ret;
+ return (1-orient)/2 * ENC_PRECISION + orient*ret;
}
// Output data to use with serial-studio
-void SerialPrint(String mode, int potent, int esc_val, uint16_t wing_angles[], float psu_vol[], float psu_cur[]) {
+void SerialPrint(String mode, int potent, int esc_val, unsigned int wing_angles[], float psu_vol[], float psu_cur[]) {
float freq = esc_val * (RpmToFreq(gMAX_RPM) / 180.0);
@@ -288,7 +297,7 @@ void SerialPrint(String mode, int potent, int esc_val, uint16_t wing_angles[], f
Serial.print(freq);
for (int i = 0; i < sizeof(gCSN_PINS); i++) {
Serial.print(",");
- Serial.print(wing_angles[i]);
+ Serial.print(PosToDeg(wing_angles[i])-gMAX_WING_POS[i] + TRUE_MAX_ANGLE);
}
for (int i = 0; i < sizeof(psu_vol); i++) {
Serial.print(",");
@@ -317,7 +326,15 @@ float GetCurrent(byte cur_pin) {
// Measure the tension provided by the PSU
float GetTension(byte vol_pin) {
- int vol_raw = analogRead(vol_pin) * (5.0 / 1023.0);
+ float vol_raw = analogRead(vol_pin) * (5.0 / 1023.0);
return vol_raw * (DIV_RES1 + DIV_RES2) / (DIV_RES2);
}
+
+// Convert encoder position to degree
+float PosToDeg(unsigned int pos){
+
+ return pos * 360.0 / ENC_PRECISION;
+
+}
+
--
GitLab