gauthiier/M-M-M
1
0
Fork 0
Code Issues 11 Pull Requests Releases Wiki Activity

Thursday Commit

Browse Source 
including David's lib from yesterday
This commit is contained in:
Bill Verplank 2013-01-24 12:11:48 +01:00
parent 5912dfdcd4
commit b74972db16
10 changed files with 136 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
software/apps/Motion/Center/Center.ino
View File

@ -14,7 +14,7 @@ int duty; // pwm duty for Timer1 (range 0 - 1023) 10-bit resolution
void setup()
{
MotorA.torque(100); //is this necessary?
MotorA.init();
}
@ -25,7 +25,7 @@ void loop()
force = k * (512 - pos);
duty = abs(force);
duty = min(1023, duty);
duty = min(512, duty);
MotorA.torque(duty);

8
software/apps/Motion/CenterA/CenterA.ino
View File

@ -8,20 +8,18 @@
int pos; // position from analogRead
int force; // computed from pos and k
int k = 1; // spring constant
int k = 10; // spring constant
int duty; // pwm duty for Timer1 (range 0 - 1023) 10-bit resolution
void setup()
{
MotorA.torque(100); //is this necessary?
MotorA.init();
}
void loop()
{
pos = analogRead(A3);
pos = analogRead(A0);
force = k * (512 - pos);
duty = abs(force);

62
software/apps/Motion/Pendulum/Pendulum.ino Normal file
View File

@ -0,0 +1,62 @@
// "Pendulum" - spring-mass oscillator
#include <Motion.h>
#include <Motor.h>
byte incomingByte;
void setup()
{
// init MotionA & MotorA
MotionA.init(INPUTA0);
MotorA.init();
// provide MotionA with initial physics constants
MotionA.k = 0.2f; // spring
MotionA.m = 0.3f; // mass
MotionA.d = 0.02f; // damping
Serial.begin(9600);
}
void loop(){
if(MotionA.F < 0) MotorA.direction(FORWARD);
else MotorA.direction(BACKWARD);
float t = abs(MotionA.F);
MotorA.torque(t);
if (Serial.available() > 0) {
incomingByte = Serial.read();
if(incomingByte == '1'){
MotionA.m = MotionA.m * 1.1;
Serial.print("m: ");
Serial.println(MotionA.m);
}
else if(incomingByte == '!'){
MotionA.m = MotionA.m / 1.1;
Serial.print("m: ");
Serial.println(MotionA.m);
}
else if(incomingByte == '2'){
MotionA.k = MotionA.k * 1.1;
Serial.print("k: ");
Serial.println(MotionA.k);
}
else if(incomingByte == '@'){
MotionA.k = MotionA.k / 1.1;
Serial.print("k: ");
Serial.println(MotionA.k);
}
else if(incomingByte == '3'){
MotionA.d = MotionA.d * 1.1;
Serial.print("d: ");
Serial.println(MotionA.d);
}
else if(incomingByte == '#'){
MotionA.d = MotionA.d / 1.1;
Serial.print("d: ");
Serial.println(MotionA.d);
}
}
}

24
software/apps/Motor/A_pos_force_test/A_pos_force_test.ino Normal file
View File

@ -0,0 +1,24 @@
//position and force reporting
//sends A0 (MotorA), A1 (ForceA)
//sends A3 (MotorB), A4 (ForceB)
// to Serial Monitor
// should FSR set-up be in MMM lib?
// Bill Verplank 24Jan13
void setup()
{
Serial.begin(9600);
//set up for FSR A1 to D4
pinMode(A1,INPUT);
pinMode(4,OUTPUT);
digitalWrite(4,LOW);
digitalWrite(A1,HIGH); //internal pull-up
}
void loop()
{
Serial.print(analogRead(A0)); //positionA
Serial.print(" ");
Serial.println(analogRead(A1)); //forceA
}

19
software/apps/Motor/Motor.ino
View File

@ -1,27 +1,22 @@
//#include <TimerOne.h>
//BV 24Jan13
#include <Motor.h>
void setup()
{
MotorA.torque(500);
MotorA.init();
MotorA.torque(500); //512 is max torque
}
void loop()
{
static int j = 1;
static long cnt = 0;
if(cnt == 50000) {
j = -1;
MotorA.direction(FORWARD);
} else if(cnt == 0) {
j = 1;
MotorA.direction(BACKWARD);
}
cnt += j;
MotorA.direction(FORWARD);
delay(1000);
MotorA.direction(BACKWARD);
delay(1000);
}

3
software/apps/Motor/motortest/motortest.ino
View File

@ -7,6 +7,7 @@ int led = 13;
void setup()
{
MotorA.init();
MotorA.torque(511); //what is max torque?
pinMode(13, OUTPUT);
}
@ -14,7 +15,9 @@ void setup()
void loop()
{
MotorA.direction(FORWARD);
digitalWrite(led,HIGH);
delay(500);
MotorA.direction(BACKWARD);
digitalWrite(led,LOW);
delay(500);
}

15
software/apps/Motor/postest/postest.ino
View File

@ -1,14 +1,21 @@
//position
//sends A0 (MotorA), A3 (MotorB) to Serial Monitor
//position and force reporting
//sends A0 (MotorA), A1 (ForceA)
//sends A3 (MotorB), A4 (ForceB)
// to Serial Monitor
void setup()
{
Serial.begin(9600);
//set up for FSR A1 to D4
pinMode(A1,INPUT);
pinMode(4,OUTPUT);
digitalWrite(4,LOW);
digitalWrite(A1,HIGH); //internal pull-up
}
void loop()
{
Serial.print(analogRead(A3));
Serial.print(analogRead(A0)); //positionA
Serial.print(" ");
Serial.println(analogRead(A0));
Serial.println(analogRead(A1)); //forceA
}

42
software/lib/MMM/Motion.cpp
View File

@ -36,9 +36,9 @@ Motion MotionB(MOTIONA);
uint8_t lb;
uint8_t hb;
int x, xx;
float T = N * 0.004f; // 4ms (see TCNT1)
float v;
int xin, dx;
float MAX_POS = 1023;
float MAX_VEL = MAX_POS / T;
@ -60,18 +60,11 @@ void Motion::init(INPUT sensor)
}
_i = true;
_s = sensor;
}
int Motion::getPosition() {
return _x;
}
float Motion::getVelocity() {
return _v;
}
float Motion::getAcceleration() {
return _a;
// initial values
d = 0;
k = 1;
m = 1;
}
void Motion::set_force_callback(force_callback fcb, PHY physics) {
@ -84,32 +77,24 @@ void Motion::set_force_callback(force_callback fcb, PHY physics) {
ISR(TIMER1_OVF_vect) {
TCNT1 = 1000;
if(MotionA._i) {
ADMUX = MotionA._s & 0x07;
ADCSRA |= (1 << ADSC);
while (ADCSRA & (1 << ADSC));
lb = ADCL;
hb = ADCH;
x = (hb << 8) | lb;
MotionA._xv[MotionA._ix] = x;
MotionA._ix++;
MotionA._ix %= N;
xx = x - MotionA._x;
if(abs(xx) < 2) {
v = 0.0f;
} else {
v = xx / T;
}
xin = (hb << 8) | lb;
MotionA.Xin = xin;
MotionA._a = (v - MotionA._v) / T;
MotionA._v = v;
MotionA._x = x;
MotionA.F = MotionA.k * (xin - MotionA.X) - (MotionA.d * MotionA.V);
//MotionA.A = MotionA.F / MotionA.m;
MotionA.V += (MotionA.F / MotionA.m) * T;
MotionA.X += MotionA.V * T;
}
/*
if(MotionB._i) {
ADMUX = MotionB._s & 0x07;
ADCSRA |= (1 << ADSC);
@ -134,6 +119,7 @@ ISR(TIMER1_OVF_vect) {
MotionB._x = x;
}
*/
}

10
software/lib/MMM/Motion.h
View File

@ -57,10 +57,6 @@ public:
void init(INPUT sensor);
int getPosition();
float getVelocity();
float getAcceleration();
void set_force_callback(force_callback fcb, PHY physics);
@ -68,9 +64,9 @@ public:
int _xv[N];
int _ix;
int _x;
float _v;
float _a;
float X, V, A, F; // from model
float m, k, d;
int Xin; // from the ADC channel specified in Motion constructor (e.g INPUTA0)
MOTION _m;
INPUT _s;

5
software/lib/MMM/Motor.cpp
View File

@ -48,16 +48,17 @@ void MMotor::init()
//direction pins are outputs
DDRD |= (1 << PD7);
DDRB |= (1 << PB0);
DDRB = (1 << PB0) | (1 << PB1) | (1 << PB2);
DDRB |= (1 << PB1) | (1 << PB2);
TCCR1A = (1 << COM1A1) | (1 << COM1B1);
// clear the bits
TCCR1B &= ~((1 << CS10) | (1 << CS11) | (1 << CS12));
TCCR1B = (1 << WGM13) | (1 << CS10);
//ICR1 = 512;
ICR1 = 512;
reg_init = true;
}