Diff: main.cpp

Revision:

10:769cc457c3a4

Parent:

9:1d9b24d7ac77

Child:

11:711d3c207e8c

--- a/main.cpp	Sat Dec 05 00:40:42 2015 +0000
+++ b/main.cpp	Sat Dec 05 09:04:23 2015 +0000
@@ -9,23 +9,33 @@
 long serialCounter = 0;
 
 
+Ticker controlsInterrupt;
+
 
 int main() {
     controls.setPC(comm.getPC());
+    controls.setup();
 //    comm.printPosition();
 //    comm.printGains();
+
+    controlsInterrupt.attach_us(&controls, &Controls::loop, 1000);
+
     while(1) {
-        controls.loop();
+        controls.updateIMUS();
         comm.check();
-        if (serialCounter++>100) {
-//            comm.printPosition();
-//            controls.printPWM();
+        if (serialCounter++>1000000) {
+//            comm.getPC()->printf("%f\n", controls.getTheta1());
+//            comm.getPC()->printf("%f", controls.motor.getPWM());
             serialCounter = 0;
+//            comm.getPC()->printf("%f\n", controls.motor.getTorque());
         }
     }
 }
 
 
+
+
+
 //wrappers for comm stack
 void openGripper1Wrapper(Arguments * input, Reply * output){
     comm.openGripper1(input, output);
@@ -51,6 +61,14 @@
     comm.setSwingUpD(input, output);
 };
 RPCFunction SetSwingUpD(&setSwingUpDWrapper, "SetSwingUpD");
+void setCurrentPWrapper(Arguments * input, Reply * output){
+    comm.setCurrentP(input, output);
+};
+RPCFunction SetCurrentP(&setCurrentPWrapper, "SetCurrentP");
+void setCurrentDWrapper(Arguments * input, Reply * output){
+    comm.setCurrentD(input, output);
+};
+RPCFunction SetCurrentD(&setCurrentDWrapper, "SetCurrentD");
 void setTargetWrapper(Arguments * input, Reply * output){
     comm.setTarget(input, output);
 };
@@ -59,8 +77,17 @@
     comm.setTorque(input, output);
 };
 RPCFunction SetTorque(&setTorqueWrapper, "SetTorque");
+void printGainsWrapper(Arguments * input, Reply * output){
+    comm.printGains();
+};
+RPCFunction PrintGains(&printGainsWrapper, "PrintGains");
+void printPositionWrapper(Arguments * input, Reply * output){
+    comm.printPosition();
+};
+RPCFunction PrintPosition(&printPositionWrapper, "PrintPosition");
 
 
 
 
 
+