63 lines
1.4 KiB
C++
63 lines
1.4 KiB
C++
#include <Servo.h>
|
|
|
|
#define SAMPLESIZE 100
|
|
|
|
int Sensor_1;
|
|
int samples[SAMPLESIZE];
|
|
int timer;
|
|
int oldTimer;
|
|
int delayBetweenOutput = 100;
|
|
int servoPin = 3;
|
|
|
|
Servo balanceScale;
|
|
|
|
void setup() {
|
|
// put your setup code here, to run once:
|
|
Serial.begin(115200);
|
|
pinMode(servoPin,OUTPUT);
|
|
balanceScale.attach(3);
|
|
//set timer2 interrupt at 2kHz
|
|
TCCR2A = 0;// set entire TCCR2A register to 0
|
|
TCCR2B = 0;// same for TCCR2B
|
|
TCNT2 = 0;//initialize counter value to 0
|
|
// set compare match register for 1khz increments
|
|
OCR2A = 250;// = (16*10^6) / (1000*64) - 1 (must be <256)
|
|
// turn on CTC mode
|
|
TCCR2A |= (1 << WGM21);
|
|
// Set CS21 and CS20 bits for 64 prescaler
|
|
TCCR2B |= (1 << CS21) | (1 << CS20);
|
|
// enable timer compare interrupt
|
|
TIMSK2 |= (1 << OCIE2A);
|
|
sei(); // Enable interrupts
|
|
timer = millis();
|
|
}
|
|
|
|
void loop() {
|
|
// put your main code here, to run repeatedly:
|
|
balanceScale.write(map(moyenne(),0,1023,180,0));
|
|
}
|
|
|
|
ISR(TIMER2_COMPA_vect){
|
|
TCNT1 = 0;
|
|
Sensor_1 = analogRead(0);
|
|
rollSamples(Sensor_1);
|
|
}
|
|
|
|
void rollSamples(int newData){
|
|
// décaler les données existantes vers la gauche
|
|
for (int i = 0; i < SAMPLESIZE - 1; i++) {
|
|
samples[i] = samples[i+1];
|
|
}
|
|
// ajouter la nouvelle donnée à la fin du tableau
|
|
samples[SAMPLESIZE - 1] = newData;
|
|
|
|
}
|
|
|
|
double moyenne(){
|
|
long all = 0;
|
|
for(int i = 0; i<SAMPLESIZE-1;i++){
|
|
all += samples[i];
|
|
}
|
|
return double(all)/double(SAMPLESIZE);
|
|
}
|