Thursday Commit

including David's lib from yesterday

software/apps/Motion/Center/Center.ino

@@ -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);
   

software/apps/Motion/CenterA/CenterA.ino

@@ -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.init();
-  MotorA.torque(100);  //is this necessary?
-  
 }
 
 void loop() 
 {
 
-  pos = analogRead(A3);
+  pos = analogRead(A0);
   
   force = k * (512 - pos);
   duty = abs(force);

software/apps/Motion/Pendulum/Pendulum.ino

@@ -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);
+    }    
+  }
+}

software/apps/Motor/A_pos_force_test/A_pos_force_test.ino

@@ -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
+}

software/apps/Motor/Motor.ino

@@ -1,27 +1,22 @@
 //#include <TimerOne.h>
+//BV 24Jan13
 
 #include <Motor.h>
 
 void setup() 
 {
-
+  MotorA.init();
-  MotorA.torque(500);  
+  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) {
+  delay(1000);
-    j = 1;
   MotorA.direction(BACKWARD);
-  }
+  delay(1000);  
-  cnt += j;  
   
 }
   

software/apps/Motor/motortest/motortest.ino

@@ -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);
 }

software/apps/Motor/postest/postest.ino

@@ -1,14 +1,21 @@
-//position
+//position and force reporting
-//sends A0 (MotorA), A3 (MotorB) to Serial Monitor
+//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
 }

software/lib/MMM/Motion.cpp

@@ -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() {
+    // initial values
-    return _x;
+    d = 0;
-}
+    k = 1;
-
+    m = 1;
-float Motion::getVelocity() {
-    return _v;
-}
-
-float Motion::getAcceleration() {
-    return _a;
 }
 
 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;
+        xin = (hb << 8) | lb;
-        if(abs(xx) < 2) {            
+        MotionA.Xin =  xin;
-            v = 0.0f;
-        } else {
-            v = xx / T;
-        }
         
-        MotionA._a = (v - MotionA._v) / T;        
+        MotionA.F = MotionA.k * (xin - MotionA.X) - (MotionA.d * MotionA.V);
-        MotionA._v = v;
+        //MotionA.A = MotionA.F / MotionA.m;
-        MotionA._x = x;       
+        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;       
         
     }
+     */
     
 }
 

software/lib/MMM/Motion.h

@@ -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 X, V, A, F;    // from model
-    float _v;
+    float m, k, d;    
-    float _a;
+    int Xin;            // from the ADC channel specified in Motion constructor (e.g INPUTA0)
         
     MOTION _m;
     INPUT  _s;

software/lib/MMM/Motor.cpp

@@ -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;
     }