Diff: Comm/Comm.cpp

Revision:

10:769cc457c3a4

Parent:

9:1d9b24d7ac77

Child:

11:711d3c207e8c

--- a/Comm/Comm.cpp	Sat Dec 05 00:40:42 2015 +0000
+++ b/Comm/Comm.cpp	Sat Dec 05 09:04:23 2015 +0000
@@ -3,7 +3,7 @@
     
 Comm::Comm(CommDelegate *controls):_pc(USBTX, USBRX), _json(&_pc)
 {
-    _pc.baud(115200);
+    _pc.baud(9600);
     _controls = controls;
 }
 
@@ -56,6 +56,27 @@
     _controls->setSwingUpD(d);
     printGains();
 }
+void Comm::setCurrentP(Arguments* input, Reply* output){
+    if (input->argc < 1){
+        throwNotEnoughArgsError();
+        return;
+    }
+    float p = input->getArg<float>(); 
+    _controls->setCurrentP(p);
+    printGains();
+}
+
+void Comm::setCurrentD(Arguments* input, Reply* output){
+    if (input->argc < 1){
+        throwNotEnoughArgsError();
+        return;
+    }
+    float d = input->getArg<float>(); 
+    _controls->setCurrentD(d);
+    printGains();
+}
+
+
 
 void Comm::printGains(){
     _json.open();
@@ -63,9 +84,9 @@
     _json.sepItem();
     _json.print("swingUpD", _controls->getSwingUpD());
     _json.sepItem();
-    _json.print("k2", _controls->getK2());
+    _json.print("currentP", _controls->getCurrentP());
     _json.sepItem();
-    _json.print("d2", _controls->getD2());
+    _json.print("currentD", _controls->getCurrentD());
     _json.close();
 }
 
@@ -90,8 +111,8 @@
         throwNotEnoughArgsError();
         return;
     }
-    float torque = atof(input->argv[0]);
-//    _pc.printf("%f", torque);
+    float torque = input->getArg<float>(); 
+    _pc.printf("%f\n", torque);
     _controls->setTorque(torque);
 }
 
@@ -103,7 +124,7 @@
 //    _json.sepItem();
     _json.print("th2", _controls->getTheta2());
 //    _json.sepItem();
-//    _json.print("dth2", controls->getDTheta2());
+//    _json.print("dth2", _controls->getDTheta2());
     _json.close();
 }