Arduino Basierter Schub/Motor Messstand.

Status: In Arbeit

• Listenpunkthttps://fpv-community.de/showthread.php?41506-Noch-ein-kleiner-Pr%FCfstand

/*****************************************************************************
  *                                                                           
  *  Copyright (c) 2013 www.der-frickler.net                   
  *                                                                           
  *                                                                           
  *  This library is free software; you can redistribute it and/or modify it  
  *  under the terms of the GNU Lesser General Public License as published    
  *  by the Free Software Foundation; either version 2 of the License, or     
  *  (at your option) any later version.                                      
  *                                                                           
  *  This software is distributed in the hope that it will be useful, but     
  *  WITHOUT ANY WARRANTY; without even the implied warranty of               
  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU        
  *  Lesser General Public License for more details.                          
  *                                                                           
  *  You should have received a copy of the GNU Lesser General Public         
  *  License along with this library; if not, write to the Free Software      
  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA  
  *                                                                           
  *****************************************************************************
  *                                                                           
  *  Arduino Thrustmeter to measure 
  *  Throttle, Volts, Amperes, Thrust, RPM, Temp and Vibration of 
  *  Motor / Propeller Combinations.
  *                                                                           
  *                                                                           
  *****************************************************************************/

#include <LiquidCrystal.h>
#include <Servo.h> 
#include "Wire.h"
#include "I2Cdev.h"
#include "BMA150.h"
#include <Hx711.h>

#define thrustPinDOUT 11
#define thrustPinSCK 12
#define currentPin A2
#define voltagePin A3
#define throttlePin 3
#define rpmPin 2

//#define CALIB_THROTTLE
//#define CALIB_VOLT_AMPERE
//#define CALIB_THRUST

// define some values used by the panel and buttons
#define btnRIGHT  0
#define btnUP     1
#define btnDOWN   2
#define btnLEFT   3
#define btnSELECT 4
#define btnNONE   5
int key, lastKey     = 5;
int adc_key_in  = 0;

// How often do we do print?
long time = 0; // 
int timeBetweenPrinting = 500; // We want a print every 500 ms;

// LCD panel
LiquidCrystal lcd(8, 9, 4, 5, 6, 7);

// scale Stuff
Hx711 scale(thrustPinDOUT, thrustPinSCK);

// Throtle Stuff
Servo throttleOut;
int throttleMS = 0; 

// ACC stuff
BMA150 accel;
int16_t ax, ay, az, acc, accAverage;
int16_t axOffset, ayOffset, azOffset;
int tempRead;
float temp;

// Voltage Stuff 
float voltsA = 0.0; // Volts
int analogVoltsA = 0; // analog reading volts
float voltsB = 14.0; // Volts
int analogVoltsB = 990; // analog reading volts
float analogVoltsAverage = 0;
int analogVoltsValue;
float volts;

// Current Stuff 
float currentA = 0.0; // Ampere
int analogCurrentA = 510; // analog reading current
float currentB = 7.01; // Ampere
int analogCurrentB = 650; // analog reading current
int analogCurrentValue;
float analogCurrentAverage = 0;
float amperes;

// Watts
float watts;

// Thrust Stuff 
long scaleOffset = 0;
float scaleScale = (1992.f/4)-35.0;
//float thrustA = 0.0; // Grams
//int analogThrustA = 170; // analog reading thrust
//float thrustB = 1000.0; // Grams
//int analogThrustB = 812; // analog reading thrust
//int analogThrustValue;
//float analogThrustAverage = 0;
float thrust;

// RPM
volatile long rpmStart;
volatile long rpm;
float rpmAverage;
int propBlades = 2;

void setup()
{

  // Init Throttle Servo
  throttleOut.attach(throttlePin);
  throttleOut.writeMicroseconds(throttleMS+1000);

  // init Serial
  Serial.begin(115200);
  Serial.println();
  Serial.println("Motorteststand - der-Frickler.net");
  Serial.println();
  
  // Init LCD 
  lcd.begin(16, 2);
  lcd.setCursor(0,0);
  lcd.print(" Motorteststand");
  lcd.setCursor(0,1);
  lcd.print("der-Frickler.net");

  // Init RPM
  attachInterrupt(0, measureRPM, RISING);
  
  // init Scale
  scale.setScale(scaleScale);

  // Init ACC / Temp
  Wire.begin();
  accel.initialize();
  // Serial.println(accel.testConnection() ? "BMA150 connection successful" : "BMA150 connection failed");
  accel.setRange(2); // +- 8g
  accel.setBandwidth(2); // 100Hz
  // calc ACC Offsets
  for (int i=0; i <= 20; i++){
    accel.getAcceleration(&ax, &ay, &az);
    axOffset += ax;
    ayOffset += ay;
    azOffset += az;
  }
  axOffset /= 20;
  ayOffset /= 20;
  azOffset /= 20;


  delay(1000);
  lcd.clear();
  
  writeHeader();

}

void writeHeader() {
    // write CSV Header
  Serial.print("Throttle(%);");
  Serial.print("Voltage(V);");
  Serial.print("Current(A);");
  Serial.print("Watts(W);");
  Serial.print("Thrust(g);");
  Serial.print("RPM(krpm);");
  Serial.print("TEMP(°C);");
  Serial.print("ACC;");
  Serial.print("Efficiency(g/W);");
  Serial.println();
}
  

void loop()
{
  // read Button
  key = read_LCD_buttons();

  // save old Keystate
  if (key != btnNONE) {
    lastKey = key;
  }
  
  if (key == btnRIGHT) {
    Serial.println();
    Serial.println();
    Serial.println();
    writeHeader();
  }

  // check for maunal inc./dec./reset
  if (key == btnUP) {
    throttleMS += 1;
  } 
  else if (key == btnDOWN) {
    throttleMS -= 1;
  } 
  else if (key == btnSELECT) {
    throttleMS = 0;
  } 

  // check for automatic inc./dec.
  if (lastKey == btnRIGHT) {
    throttleMS += 1; 
  } 
  else if (lastKey == btnLEFT) {
    throttleMS -= 1; 
  }
  
//  if (throttleMS < 1000) {
//    Serial.println();
//    Serial.println();
//    Serial.println();
//    writeHeader();
//  }

  // limit Throttle
  throttleMS = constrain(throttleMS, 0, 1000);

  // Start Measuring

  // get volts
  analogVoltsValue = analogRead(voltagePin);
  analogVoltsAverage = 0.5*analogVoltsAverage + 0.5*analogVoltsValue;

  // get current
  int curAvCount = 10; // sample current several times
  analogCurrentValue = 0;
  for (int i=0; i < curAvCount; i++){
    analogCurrentValue += analogRead(currentPin);  
    //Serial.println(analogCurrentValue);
  }
  analogCurrentValue = analogCurrentValue/curAvCount;
  //Serial.println(analogCurrentValue);
  //Serial.println();
  analogCurrentAverage = 0.5*analogCurrentAverage + 0.5*analogCurrentValue;
  
//  // get thrust
//  analogThrustValue = analogRead(thrustPin);
//  analogThrustAverage = 0.5*analogThrustAverage + 0.5*analogThrustValue;

  // get ACC and Temp
  accel.getAcceleration(&ax, &ay, &az);
  ax -= axOffset;
  ay -= ayOffset;
  az -= azOffset;
  acc = max(abs(ax), abs(ay));
  acc = max(acc, abs(az));
  accAverage = 0.8*accAverage + 0.2*acc;
  tempRead = accel.getTemperature();
  temp = 0.8*temp + 0.2*((tempRead * 0.5) - 30.0);

  // get RPM
  rpmAverage = 0.8*rpmAverage + 0.2*rpm;


  // set throttle
  throttleOut.writeMicroseconds(throttleMS+1000);

#ifdef CALIB_VOLT_AMPERE
  lcd.clear();

  lcd.setCursor(0,0);
  lcd.print(analogVoltsAverage);
  lcd.setCursor(8,0);
  lcd.print(analogToVolts(analogVoltsAverage),1);
  lcd.print("V"); 

  lcd.setCursor(0,1);
  lcd.print(analogCurrentAverage);
  lcd.setCursor(8,1);
  lcd.print(analogToAmperes(analogCurrentAverage),1);
  lcd.print("A"); 

  delay(250);  
#endif
#ifdef CALIB_THRUST

  //thrust = analogToThrust(analogThrustAverage);
  thrust = scale.getGram();
  
  lcd.clear();
  lcd.setCursor(0,0);
//  lcd.print(analogVoltsAverage);
//  lcd.setCursor(8,0);
  lcd.print(thrust,1);
  lcd.print(" Gram"); 


  delay(250);  
#endif
#if  !defined (CALIB_VOLT_AMPERE)  && !defined (CALIB_THRUST) 


  // Is it time to print? 
  if(millis() > time + timeBetweenPrinting) {
    // print throttle
    lcd.setCursor(0,0);
    if (throttleMS < 1000) lcd.print(" ");
    if (throttleMS < 100) lcd.print(" ");
    lcd.print(throttleMS/10); 
    lcd.print("% "); 

    // print Temp
//      if (temp < 10) lcd.print(" ");
//      lcd.print(temp,1); 
//      lcd.print("C "); 
    
    // Print Thrust
    //thrust = analogToThrust(analogThrustAverage);
    thrust = scale.getGram();
    //thrust = analogToThrust(analogThrustValue);
    if (thrust < 0) thrust = 0;
    //if (thrust < 1000) lcd.print(" ");
    if (thrust < 100) lcd.print(" ");
    if (thrust < 10) lcd.print(" ");
    lcd.print(thrust,1); 
    lcd.print("g "); 

    // Print RPM
    //if (rpm < 1000) lcd.print(" ");
    if (rpm < 100) lcd.print(" ");
    if (rpm < 10) lcd.print(" ");
    lcd.print(rpm,1); 
    lcd.print("R "); 

    // Print ACC
//    if (accAverage < 10) lcd.print(" ");
//    lcd.print(accAverage,1); 
//    lcd.print("G "); 

    // 2nd Line
    lcd.setCursor(0,1);

    // print voltage
    volts = analogToVolts(analogVoltsAverage);
    if (volts < 10) lcd.print(" ");
    lcd.print(volts,1); 
    lcd.print("V"); 

    // print current
    amperes = analogToAmperes(analogCurrentValue);
    if (amperes < 0 ) amperes = 0.0;
    if (amperes < 10) lcd.print(" ");
    lcd.print(amperes,1); 
    lcd.print("A"); 

    // Print Wat
    watts = volts * amperes;
    if (watts < 100) lcd.print(" ");
    if (watts < 10) lcd.print(" ");
    lcd.print(watts,1); 
    lcd.print("W"); 


if (throttleMS > 0) {

    // write Values to Serial as CSV
    Serial.print(throttleMS/10);
    Serial.print(";");
    Serial.print(volts);
    Serial.print(";");
    Serial.print(amperes);
    Serial.print(";");
    Serial.print(watts);
    Serial.print(";");
    Serial.print(thrust);
    Serial.print(";");
    Serial.print(rpm/1000);
    Serial.print(";");
    Serial.print(temp);
    Serial.print(";");
    Serial.print(accAverage);
    Serial.print(";");
    Serial.print(thrust/watts);
    Serial.print(";");
    Serial.println();
}

    time = millis();
  }

  delay(50); 

#endif

}


// read the buttons
int read_LCD_buttons()
{
  adc_key_in = analogRead(0);      // read the value from the sensor
  // my buttons when read are centered at these valies: 0, 144, 329, 504, 741
  // we add approx 50 to those values and check to see if we are close
  if (adc_key_in > 1000) return btnNONE; // We make this the 1st option for speed reasons since it will be the most likely result
  // For V1.1 us this threshold
  if (adc_key_in < 50)   return btnRIGHT; 
  if (adc_key_in < 250)  return btnUP;
  if (adc_key_in < 450)  return btnDOWN;
  if (adc_key_in < 650)  return btnLEFT;
  if (adc_key_in < 850)  return btnSELECT; 

  // For V1.0 comment the other threshold and use the one below:
  /*
 if (adc_key_in < 50)   return btnRIGHT; 
   if (adc_key_in < 195)  return btnUP;
   if (adc_key_in < 380)  return btnDOWN;
   if (adc_key_in < 555)  return btnLEFT;
   if (adc_key_in < 790)  return btnSELECT;  
   */
  return btnNONE;  // when all others fail, return this...
}


float mapfloat(float x, float in_min, float in_max, float out_min, float out_max)
{
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}


float analogToVolts(float analogval){
  float load = mapfloat(analogval, analogVoltsA, analogVoltsB, voltsA, voltsB);
  return load;
}

float analogToAmperes(float analogval){
  float load = mapfloat(analogval, analogCurrentA, analogCurrentB, currentA, currentB);
  return load;
}

//float analogToThrust(float analogval){
//  float load = mapfloat(analogval, analogThrustA, analogThrustB, thrustA, thrustB);
//  return load;
//}

void measureRPM() {
  rpm = 60000000 / ((micros() - rpmStart) * propBlades);
  rpmStart = micros();
}