Revision 10:e328acbdf885, committed 2017-11-07

Comitter:

maaikelaagland

Date:

Tue Nov 07 09:07:01 2017 +0000

Parent:

9:5023f5a21eab

Commit message:

MBED 21 script

Changed in this revision

diff -r 5023f5a21eab -r e328acbdf885 main.cpp
--- a/main.cpp	Thu Nov 02 19:48:09 2017 +0000
+++ b/main.cpp	Tue Nov 07 09:07:01 2017 +0000
@@ -1,7 +1,6 @@
 #include "QEI.h"
 #include "math.h"
 #include "mbed.h"
-//#include "HIDScope.h" //set mbed library version to 119 for HIDScope to work
 #include "MODSERIAL.h"
 #include "BiQuad.h"
 
@@ -11,9 +10,6 @@
 
 //--------------object creation------------------ 
 Serial pc(USBTX, USBRX);
-//Use X4 encoding.
-//QEI wheel(p29, p30, NC, 624, QEI::X4_ENCODING);
-//Use X2 encoding by default.
 QEI enc1(D13, D12, NC, 32); //enable the encoder
 QEI enc2(D15, D14, NC, 32); //enable the encoder
 PwmOut M1_speed(D6);
@@ -28,8 +24,6 @@
 DigitalIn HomStart(D3); // - left button
 
 BiQuad bqlowpass(0.4354, 0.8709, 0.4354, 0.5179, 0.2238);
-//create scope objects - note: script won't run when HID usb port is not connected
-//HIDScope scope(5); //set # of channels
 
 Ticker motor_update1;
 Ticker motor_update2;
@@ -90,7 +84,6 @@
   double q2_dot = -(0.3*cos(q1+q2)+0.4*cos(q1))/(0.12*sin(q2))*vdx -(0.3*sin(q1+q2)+0.4*sin(q1))/(0.12*sin(q2))*vdy;
   q1_new += q1_dot*Ts;
   q2_new += (q2_dot-q1_dot)*Ts;
-  //pc.printf("q1 dot = %f, q2 dot = %f, ",q1_dot,q2_dot);
  return;
 }
 
@@ -129,8 +122,6 @@
      M2_direction.write(1);
      }
     else{M2_speed.write(0);}       
-    //pc.printf("M2 speed = %f, M2 direction = %i, M2 pos error =  %f, M2 int error = %f\n\r",set_speed,M2_direction.read(),M2_error_pos,M2_e_int);
-    //pc.printf("M2 integral = %f position error = %f\n\r", M2_e_int,M2_error_pos);
   }
  
  void homing_system () {
@@ -292,9 +283,6 @@
     
     q1_step = q1;
     q2_step = q2;
-   //stop motor interrupt
-//motor_update1.attach(&M1_control,0.01);
-//motor_update2.attach(&M2_control,0.01);
 error_update.attach(&geterror,0.02);
 
 pc.printf("initialization complete - position control of motors now active\n\r");