77 lines
1.3 KiB
C++
77 lines
1.3 KiB
C++
#include <Servo.h>
|
|
Servo Scale;
|
|
|
|
int outputPinServoMotor = 10;
|
|
int const sampleSize = 50;
|
|
double sampleArray[sampleSize];
|
|
int const inputPinSensor = 0;
|
|
|
|
void setup() {
|
|
Serial.begin(115200);
|
|
Scale.attach(outputPinServoMotor);
|
|
|
|
/*TCCR2A = 0;
|
|
TCCR2B = 0;
|
|
TCNT2 = 0;
|
|
|
|
OCR2A = 250;
|
|
TCCR2A |= (1 << WGM21);
|
|
TCCR2B |= (1 << CS21) | (1 << CS20);
|
|
TIMSK2 |= (1 << OCIE2A);
|
|
sei();*/
|
|
|
|
}
|
|
|
|
void loop() {
|
|
// put your main code here, to run repeatedly:
|
|
|
|
|
|
delay(10);
|
|
int val = analogRead(inputPinSensor);
|
|
addval(val);
|
|
|
|
|
|
int servoVal = sensorValToServoVal(getAVG());
|
|
Serial.println(servoVal);
|
|
|
|
Scale.write(servoVal);
|
|
|
|
}
|
|
|
|
/*ISR(TIMER2_COMPA_vect){
|
|
TCNT1 = 0;
|
|
int val = analogRead(inputPinSensor);
|
|
addval(val);
|
|
//Serial.println(val);
|
|
}*/
|
|
|
|
void addval(int val)
|
|
{
|
|
// we need to add the val at the start of the array
|
|
for (int i=0; i<sampleSize-1; i++)
|
|
{
|
|
sampleArray[i+1] = sampleArray[i];
|
|
}
|
|
sampleArray[0] = val;
|
|
}
|
|
|
|
int sensorValToServoVal(double sensorVal)
|
|
{
|
|
|
|
//float y = -272.6*log(sensorVal)+1103.9;
|
|
//float x = 851.99*exp(-0.054*sensorVal);
|
|
//Serial.println(x);
|
|
|
|
return map(sensorVal, 0, 1023, 0, 180);
|
|
}
|
|
|
|
double getAVG()
|
|
{
|
|
double avg = 0;
|
|
for (int val: sampleArray)
|
|
{
|
|
avg += (double) val/(double)sampleSize;
|
|
}
|
|
return avg;
|
|
}
|