Revision 38:922f2584bdfd, committed 2013-11-26

Comitter:

calamaridudeman

Date:

Tue Nov 26 04:42:39 2013 +0000

Parent:

37:bf257a0154db

Child:

41:65de628f701f

Commit message:

motors now move! Unknown cause, pwm freq is now being set just before every setting of duty cycle, not sure why this is needed

Changed in this revision

--- a/Master.cpp	Sun Nov 24 20:38:59 2013 +0000
+++ b/Master.cpp	Tue Nov 26 04:42:39 2013 +0000
@@ -7,21 +7,48 @@
 QEI mEnc1(p25, p26, NC, 1200, QEI::X4_ENCODING); //hip
 QEI mEnc2(p23, p24, NC, 1200, QEI::X4_ENCODING);//knee
 
-Motor m1(p15,p17,p18,p21,mEnc1);//hip
-Motor m2(p16,p19,p20,p22,mEnc2);//knee
+Motor m1(p15,p17,p18,p21,mEnc2);//hip p17 high is CCW
+Motor m2(p16,p19,p20,p22,mEnc1);//knee p19 high is CCW
 
-QEI bEnc1(p27, p28, p29, 1200, QEI::X4_ENCODING);  //track
-QEI bEnc2(p8, p9, p10, 1200, QEI::X4_ENCODING); //body
+QEI bEnc1(p27, p28, p29, 500, QEI::X4_ENCODING);  //track  offset:-2.236814
+QEI bEnc2(p5, p6, p7, 500, QEI::X4_ENCODING); //body   offset:1.770973
 
 Kangaroo kankan(m1,m2,bEnc1,bEnc2);
 
+Ticker t;
+
+AnalogIn aIn(p15);
+        DigitalOut Forward(p17);
+        DigitalOut Backward(p18);
+        PwmOut pwmOut(p21);
+
 int main() {
     
-    while(1){
-        kankan.testEncoders(pc);
+    /*while(1){
+        //kankan.testEncoders(pc);
+        pc.printf("%f\n", kankan.getAngle());
         wait(.25);
-    }
+    }*/
+    
+    m1.setPos(0.0);
+    m1.start();
+    //t.attach(&m1, &Motor::Control, .0005);
+    wait(5);
+    m1.setPos(3.1415/6);
+    wait(5);
+    m1.stop();
     
+    /*Forward=1;
+        Backward=0;
+            pwmOut.period_us(500);
+
+    
+    pwmOut.write(.15);
+    
+    wait(5);
+    pwmOut.write(0);
+    
+    */
     
     //kankan.zero();
 }

--- a/QEI.lib	Sun Nov 24 20:38:59 2013 +0000
+++ b/QEI.lib	Tue Nov 26 04:42:39 2013 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/calamaridudeman/code/QEI/#865d5b989ec4
+http://mbed.org/users/calamaridudeman/code/QEI/#c10e8ecc2e87

--- a/include/kangaroo.hpp	Sun Nov 24 20:38:59 2013 +0000
+++ b/include/kangaroo.hpp	Tue Nov 26 04:42:39 2013 +0000
@@ -15,6 +15,7 @@
         void run();
         void stop();
         void updatePose();
+        float getAngle();
         void testEncoders(Serial &pc);
         
     private:

--- a/include/motor.hpp	Sun Nov 24 20:38:59 2013 +0000
+++ b/include/motor.hpp	Tue Nov 26 04:42:39 2013 +0000
@@ -38,7 +38,7 @@
         float angle;
         float voltage;
         int mode;
-        
+                
         float dAngularVelocity;
         float dAngle;
         float dTorque;

--- a/src/dynamics.cpp	Sun Nov 24 20:38:59 2013 +0000
+++ b/src/dynamics.cpp	Tue Nov 26 04:42:39 2013 +0000
@@ -8,4 +8,5 @@
     float theta1 = (float) 2 * atan(sqrt( ( (l2+l3+l4)*(l2+l3+l4) - x*x - y*y) / (x*x + y*y - (l2+l4-l3)*(l2+l4-l3)) ));
     float theta2 = (float) atan2(y,x) - atan2(l3*sin(theta1), l2 + l4 + l3*cos(theta1));
     return Joints(theta1, theta2);
-}
\ No newline at end of file
+}
+

--- a/src/kangaroo.cpp	Sun Nov 24 20:38:59 2013 +0000
+++ b/src/kangaroo.cpp	Tue Nov 26 04:42:39 2013 +0000
@@ -7,10 +7,16 @@
 }
 
 void Kangaroo::zero(){
-    m1.setVel(-.002);//wait for the legs to go to stopping points
-    m2.setVel(-.002);
+    m1.start();
+    m2.start();
+    //m1.setVel(-.2);//wait for the legs to go to stopping points
+    //m2.setVel(-.2);
+    
+    m1.setPos(2.0);
+    m2.setPos(2.0);
+    
     led4=1;
-    wait(1);
+    wait(3.0);
     
     m1.zero();
     m2.zero();
@@ -24,7 +30,8 @@
     
     led1=1;
     led3=1;
-    
+    m1.stop();
+    m2.stop();
 }
 
 void Kangaroo::testEncoders(Serial &pc){
@@ -35,4 +42,8 @@
     
     //pc.printf("motor2:  %i\n",);
     pc.printf("clix2:  %i, rots2: %i\n",enc2.getPulses(), enc2.getRevolutions());
+}
+
+float Kangaroo::getAngle(){
+    return (enc2.getAngle())+1.770973;
 }
\ No newline at end of file

--- a/src/motor.cpp	Sun Nov 24 20:38:59 2013 +0000
+++ b/src/motor.cpp	Tue Nov 26 04:42:39 2013 +0000
@@ -3,14 +3,15 @@
 
 Motor::Motor(PinName aPin, PinName fPin, PinName bPin, PinName pwmPin, QEI &enc):aIn(aPin),Forward(fPin),Backward(bPin),pwmOut(pwmPin),encoder(enc){ 
 //15,17,18,21,qei25,26 and 16,19,20,22,qei23,24
-    freq =.001;
+    freq =.0005;
+    speed=0;
     voltage = 12.0;
     mode=0;
     
     kp=3;
     kd=0.04;
     
-    pwmOut.period_us(.0005);
+    pwmOut.period_us(100);
     }
 
 void Motor::start(){
@@ -33,18 +34,20 @@
     angle = encoder.getPulses()/1200.0*6.283; // Updating new motor angle
     speed =Motor::filterLowPass(speed,(angle-preAngle)/freq, .2); // Updating new motor angular vel
     float mCurrent = getCurrent();
+    //pc.printf("current: %f  speed: %f\n", mCurrent, speed);
     
     float dCurrent;
     switch(mode){
+        case 0:
+            dCurrent = dTorque/3.27;
         case 1:
             dCurrent = kp*(dAngle-angle);
         case 2:
             dCurrent = kd*(dAngularVelocity-speed);
-        case 3:
+        default:
             dCurrent = kp*(dAngle-angle)+kd*(dAngularVelocity-speed);
-        case 0:
-            dCurrent = dTorque/3.27;
     }    
+
     
     float duty=0;
     if (dCurrent>0){
@@ -60,13 +63,15 @@
     if (duty>0.99){
         duty =1;
     }
+    pwmOut.period_us(100);
+    pwmOut.write(duty);
     
-    pwmOut.write(duty);
+
 }
 
 float Motor::getCurrent()
 {
-    return (1/.140)*3.3 * aIn;
+    return (1/.140)*3.3 * aIn.read();
 }
 
 int Motor::getPos()
@@ -84,8 +89,9 @@
 }
 
 void Motor::setPos(float pos){
-    mode = 1;
+    mode = 3;
     dAngle=pos;
+    dAngularVelocity = 0;
 }
 
 void Motor::setVel(float vel){