52 lines
958 B
C++
52 lines
958 B
C++
//PluckOverlapHalf
|
|
// BV 3 Feb 13
|
|
// one pluck at x=512
|
|
|
|
int x, fout, count;
|
|
float f;
|
|
float w = 50; //width of pluck
|
|
float h = 500; //height of pluck
|
|
float slope = h/w;
|
|
boolean forward = true;
|
|
|
|
#include <Motor.h>
|
|
|
|
void setup(){
|
|
MotorA.init();
|
|
MotorB.init();
|
|
Serial.begin(9600);
|
|
}
|
|
|
|
void loop(){
|
|
//x = analogRead(A0) - 512;
|
|
x = analogRead(A3) - 512;
|
|
if (forward){
|
|
if (x <= - w/2) f = 0;
|
|
if (x > -w/2 && x< w/2) f = - slope*(x + w/2);
|
|
if (x > w/2){
|
|
f = 0;
|
|
forward = false;
|
|
Serial.println("pluck forward");
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (x > w/2) f = 0;
|
|
if (x < w/2 && x > -w/2) f = - slope*(x - w/2);
|
|
if (x < -w/2){
|
|
f = 0;
|
|
forward = true;
|
|
Serial.println("pluck back");
|
|
}
|
|
}
|
|
fout = int(f);
|
|
//MotorA.torque(fout);
|
|
MotorB.torque(fout);
|
|
if(count++>=0){
|
|
count=-1000; // wait 1000 loops before print
|
|
Serial.print(x,DEC);
|
|
Serial.print(" ");
|
|
Serial.println(fout,DEC);
|
|
}
|
|
}
|