35 lines
623 B
C++
35 lines
623 B
C++
//CenterFSR-A
|
|
//position on A0, pwm:D9, dir:D8,D7
|
|
//FSR on A1 and D4
|
|
|
|
// BUZZ! (float vs int?)
|
|
#include "Motor.h"
|
|
|
|
int x, duty, fin;
|
|
float fout;
|
|
byte count; // used as counter
|
|
|
|
void setup(){
|
|
Serial.begin(9600);
|
|
MotorA.init();
|
|
//for FSR
|
|
pinMode(A1,INPUT);
|
|
digitalWrite(A1,HIGH);
|
|
pinMode(4,OUTPUT);
|
|
digitalWrite(4,LOW);
|
|
}
|
|
|
|
void loop(){
|
|
fin = 1024 - analogRead(A1);
|
|
x = analogRead(A0)-512;
|
|
fout = - .004*(float(fin)*float(x));
|
|
MotorA.torque(fout);
|
|
if(count++ == 0){
|
|
Serial.print(x,DEC);
|
|
Serial.print(" ");
|
|
Serial.print(fin,DEC);
|
|
Serial.print(" ");
|
|
Serial.println(fout,DEC);
|
|
}
|
|
}
|