📣ความรู้ไปประยุกต์ใช้อุปกรณ์อิเล็กทรอนิกส์เพื่อสร้างสรรค์เป็นโครงงานตามที่โจทย์กำหนดได้ คำสั่งเขียน
#include <Servo.h>
Servo myservo1; //create servo object
Servo myservo2; //create servo object
Servo myservo3; //create servo object
//EMA stands for (Exponential moving average)
//Used pins
//accelerometer
const int x_pin = A0;
const int y_pin = A1;
const int z_pin = A2;
//Potentiometer
const int gx_pin = A3;
const int gy_pin = A4;
const int gz_pin = A5;
//Servo
const int servoPin1 = 3;
const int servoPin2 = 5;
const int servoPin3 = 11;
//variables
//initialization of sensor variable, equibalaent
int sensorValue1 = 0;
int sensorValue2 = 0;
int sensorValue3 = 0;
//EMA
float EMA_a = 0.02; //initialization of EMA alpha
int EMA_S1 = 0; //initialization of EMA S1
int EMA_S1_map = 0; //initialization of variable for servo control
int EMA_S2 = 0; //initialization of EMA S2
int EMA_S2_map = 0; //initialization of variable for servo control
int EMA_S3 = 0; //initialization of EMA S3
int EMA_S3_map = 0; //initialization of variable for servo control
int angleX = 0;
int angleY = 0;
int angleZ = 0;
// time
unsigned long startTime = 0;
unsigned long currentTime = 0;
unsigned long elapsedTime = 0;
// Volts per G-Force
const float sensitivity = 0.206;
void setup() {
Serial.begin(115200);
analogReference(EXTERNAL);
EMA_S1 = analogRead(x_pin); //set EMA S for t = 1
EMA_S2 = analogRead(y_pin); //set EMA S for t = 1
EMA_S3 = analogRead(z_pin); //set EMA S for t = 1
myservo1.attach(servoPin1);
myservo2.attach(servoPin2);
myservo3.attach(servoPin3);
}
void loop() {
currentTime = millis();
elapsedTime = currentTime - startTime;
// Read pins and convert to G
// accelerometer rate
float ax = (analogRead(x_pin) - 512) * 3.3 / (sensitivity * 1023);
float ay = (analogRead(y_pin) - 512) * 3.3 / (sensitivity * 1023);
float az = (analogRead(z_pin) - 512) * 3.3 / (sensitivity * 1023);
// gyroscope rate
float gx = (analogRead(gx_pin) - 512) * 3.3 / (sensitivity * 1023);
float gy = (analogRead(gy_pin) - 512) * 3.3 / (sensitivity * 1023);
float gz = (analogRead(gz_pin) - 512) * 3.3 / (sensitivity * 1023);
// serva 1
move_motor(x_pin, EMA_S1, EMA_S1_map, sensorValue1, gx);
angleX = complementaryFilter(angleX, gx, elapsedTime, EMA_S1);
myservo1.write(angleX);//send the latest value to the servo
// myservo1.write(EMA_S1_map);//send the latest value to the servo
// serva 2
move_motor(y_pin, EMA_S2, EMA_S2_map, sensorValue2, gy);
angleY = complementaryFilter(angleY, gy, elapsedTime, EMA_S2);
myservo2.write(angleX);//send the latest value to the servo
//myservo2.write(EMA_S2_map);//send the latest value to the servo
// serva 3
move_motor(z_pin, EMA_S3, EMA_S3_map, sensorValue3, gz);
angleZ = complementaryFilter(angleZ,gz, elapsedTime, EMA_S3);
myservo3.write(angleZ);//send the latest value to the servo
// myservo3.write(EMA_S3_map);//send the latest value to the servo
delay(100);
startTime = currentTime;
}
void move_motor(int input_pin, int &EMA_S, int &EMA_S_map, int &sensorValue, float gyroVar){
sensorValue = analogRead(input_pin); //read the sensor
EMA_S = (EMA_a*sensorValue) + ((1-EMA_a)*EMA_S);//run the EMA
Serial.print("pin ");
Serial.print(input_pin);
Serial.print(": ");
Serial.print(sensorValue);//the first variable for plotting
Serial.print(",");//seperator
Serial.println(EMA_S);//the second variable for plotting including line break
EMA_S_map = map(EMA_S, 0, 1023, 0, 180);//map ADC values to servo values (0-180)
};
//Complementary Filter, similar to exponential, this example is for an accelerometer
// The 0.98 and 0.02 are the same as the weights in the exponential filter
//angle = 0.98 *(angle+gyro*dt) + 0.02*acc
float complementaryFilter(float angle, float gyro, float dt, float acc)
{
float totalAngle = .5 * (angle + gyro * dt) + .5 * acc;
return totalAngle;
}
📣ความรู้ไปประยุกต์ใช้อุปกรณ์อิเล็กทรอนิกส์เพื่อสร้างสรรค์เป็นโครงงานตามที่โจทย์กำหนดได้ วงจร
.jpg)
ไม่มีความคิดเห็น:
แสดงความคิดเห็น
ครูเต้ง เทคโนฯ