100 lines
3.4 KiB
C++
100 lines
3.4 KiB
C++
/**
|
|
* @file Interpolation_FlexiForce_Estimation.ino
|
|
* @author Charles STELANDRE
|
|
* @version V0.1
|
|
* @date 2025/05/11
|
|
* @brief Description: this code estimates the mass from a FlexiForce A201 sensor.
|
|
* First, you must record the signal within a range of [0-5]V, and the mass put on the sensor
|
|
* Second, you must report this data in the calibrationData. You must do so in an ascending order.
|
|
* Test the full range. Masses outside of the range you tested may not give the required result.
|
|
*
|
|
*
|
|
* Function List:
|
|
* 1. double getVoltage(int rawValue)
|
|
* 2. double getMass(double voltage)
|
|
* 3. void setup()
|
|
* 4. void loop()
|
|
*/
|
|
|
|
#include <Servo.h>
|
|
|
|
Servo myServo; // Create a servo object
|
|
const int sensorPin = A0; // Analog pin for the weight sensor
|
|
const int servoPin = 9; // Digital pin for the servo
|
|
|
|
// Calibration data: {voltage_in_V, mass_in_grams}
|
|
//Enter your calibration data here.
|
|
double calibrationData[][2] = {
|
|
{1.776, 500},
|
|
{3.288, 200},
|
|
{4.076, 100},
|
|
{4.530, 50},
|
|
{4.824, 20},
|
|
{4.924, 10},
|
|
{4.92, 0},
|
|
};
|
|
const int calibrationDataSize = sizeof(calibrationData) / sizeof(calibrationData[0]);
|
|
|
|
// Function to convert raw analog reading (0-1023) to voltage (0-5V)
|
|
double getVoltage(int rawValue) {
|
|
return (rawValue * 5.0) / 1023.0;
|
|
}
|
|
|
|
// Function to map the sensor voltage to mass in grams using linear interpolation
|
|
double getMass(double voltage) {
|
|
if (voltage <= calibrationData[0][0]) {
|
|
return calibrationData[0][1];
|
|
}
|
|
if (voltage >= calibrationData[calibrationDataSize - 1][0]) {
|
|
return calibrationData[calibrationDataSize - 1][1];
|
|
}
|
|
|
|
for (int i = 0; i < calibrationDataSize - 1; i++) {
|
|
if (voltage >= calibrationData[i][0] && voltage < calibrationData[i + 1][0]) {
|
|
return calibrationData[i][1] + (voltage - calibrationData[i][0]) *
|
|
(calibrationData[i + 1][1] - calibrationData[i][1]) /
|
|
(calibrationData[i + 1][0] - calibrationData[i][0]);
|
|
}
|
|
}
|
|
return -1; // Should not happen if data is sorted
|
|
}
|
|
|
|
// Function to map the mass (0-500g initially, scaling to 5kg later) to servo angle (0-180 degrees)
|
|
int massToAngle(double mass) {
|
|
// Assuming the servo has a 180-degree range (can increase to show when out of range).
|
|
// We'll map the current tested range (0-500g) to the full servo range.
|
|
// Once you have data up to 5kg, you might need to adjust this mapping.
|
|
return map(mass, 0, 500, 0, 180); // Change 0 and 500 to your calibration points' limits if needed
|
|
}
|
|
|
|
void setup() {
|
|
Serial.begin(9600);
|
|
myServo.attach(servoPin); // Attaches the servo on pin 9 to the servo object
|
|
Serial.println("Weight Sensor Test Sketch (Voltage Input)");
|
|
}
|
|
|
|
void loop() {
|
|
int rawValue = analogRead(sensorPin);
|
|
double currentVoltage = getVoltage(rawValue);
|
|
double currentMass = getMass(currentVoltage);
|
|
|
|
Serial.print("Raw Sensor Value: ");
|
|
Serial.print(rawValue);
|
|
Serial.print(" | Voltage: ");
|
|
Serial.print(currentVoltage);
|
|
Serial.print(" V | Estimated Mass: ");
|
|
Serial.print(currentMass);
|
|
Serial.println(" g");
|
|
|
|
if (currentMass != -1) {
|
|
int servoAngle = massToAngle(currentMass);
|
|
servoAngle = constrain(servoAngle, 0, 180); // Ensure the angle is within the servo's limits
|
|
myServo.write(servoAngle);
|
|
Serial.print("Servo Angle: ");
|
|
Serial.println(servoAngle);
|
|
} else {
|
|
Serial.println("Error: Sensor voltage out of calibration range.");
|
|
}
|
|
|
|
delay(100); // Adjust delay as needed
|
|
} |