83 lines
1.5 KiB
Arduino
83 lines
1.5 KiB
Arduino
|
#include <Motion.h>
|
||
|
|
#include <Motor.h>
|
||
|
|
#include <Music.h>
|
||
|
|
|
||
|
|
char buf[16] = "";
|
||
|
|
|
||
|
|
char b = 'x';
|
||
|
|
String inputString = "";
|
||
|
|
boolean stringComplete = false;
|
||
|
|
|
||
|
|
float k, m, d;
|
||
|
|
|
||
|
|
int cnt = 0;
|
||
|
|
|
||
|
|
void setup() {
|
||
|
|
|
||
|
|
// MOTOR
|
||
|
|
MotorA.init();
|
||
|
|
|
||
|
|
// MUSIC
|
||
|
|
Music.init();
|
||
|
|
|
||
|
|
// MOTION
|
||
|
|
MotionA.init(INPUTA0);
|
||
|
|
MotionA.k = 5.2f; // spring
|
||
|
|
MotionA.m = 1.0f; // mass
|
||
|
|
MotionA.d = 8.02f; // damping
|
||
|
|
|
||
|
|
// Serial
|
||
|
|
Serial.begin(9600);
|
||
|
|
}
|
||
|
|
|
||
|
|
void loop() {
|
||
|
|
|
||
|
|
MotionA.update_mass_spring_damper();
|
||
|
|
|
||
|
|
MotorA.torque(MotionA.F);
|
||
|
|
|
||
|
|
int f = map(abs(MotionA.F), 0, 512, 64, 76);
|
||
|
|
Music.noteOn(f);
|
||
|
|
|
||
|
|
cnt++;
|
||
|
|
|
||
|
|
if(cnt == 10) {
|
||
|
|
sprintf(buf, "%d %d %d %d", (int)MotionA.F, (int)MotionA.V, (int)MotionA.X, (int)MotionA.Xin);
|
||
|
|
Serial.println(buf);
|
||
|
|
cnt = 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
if(stringComplete) {
|
||
|
|
if(b == 'k') {
|
||
|
|
MotionA.k = convertToFloat(inputString);
|
||
|
|
} else if(b == 'm') {
|
||
|
|
MotionA.m = convertToFloat(inputString);
|
||
|
|
} else if(b == 'd') {
|
||
|
|
MotionA.d = convertToFloat(inputString);
|
||
|
|
}
|
||
|
|
b = 'x';
|
||
|
|
stringComplete = false;
|
||
|
|
inputString = "";
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void serialEvent() {
|
||
|
|
while (Serial.available()) {
|
||
|
|
char inChar = (char)Serial.read();
|
||
|
|
if(inChar == 'k' || inChar == 'm' || inChar == 'd') {
|
||
|
|
b = inChar;
|
||
|
|
} else {
|
||
|
|
if (inChar == ';') {
|
||
|
|
stringComplete = true;
|
||
|
|
} else
|
||
|
|
inputString += inChar;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
float convertToFloat(String str) {
|
||
|
|
char buf[16] = "";
|
||
|
|
str.toCharArray(buf, str.length() + 1);
|
||
|
|
return atof(buf);
|
||
|
|
}
|