Comm.cpp

00001 #include "Comm.h"
00002 
00003     
00004 Comm::Comm(CommDelegate *controls):_pc(USBTX, USBRX), _json(&_pc)
00005 {
00006     _pc.baud(115200);
00007     _controls = controls;
00008 }
00009 
00010 void Comm::openGripper1(Arguments* input, Reply* output){
00011     printGripper1State(true);
00012 }
00013 
00014 void Comm::closeGripper1(Arguments* input, Reply* output){
00015     printGripper1State(false);
00016 }
00017 
00018 void Comm::printGripper1State(bool state){
00019     _json.open();
00020     if (state) _json.print("gripper1", "true");
00021     else _json.print("gripper1", "false");
00022     _json.close();
00023 }
00024 
00025 void Comm::openGripper2(Arguments* input, Reply* output){
00026     printGripper2State(true);
00027 }
00028 
00029 void Comm::closeGripper2(Arguments* input, Reply* output){
00030     printGripper2State(false);
00031 }
00032 
00033 void Comm::printGripper2State(bool state){
00034     _json.open();
00035     if (state) _json.print("gripper2", "true");
00036     else _json.print("gripper2", "false");
00037     _json.close();
00038 }
00039 
00040 void Comm::setSwingUpK(Arguments* input, Reply* output){
00041     if (input->argc < 1){
00042         throwNotEnoughArgsError();
00043         return;
00044     }
00045     float k = input->getArg<float>(); 
00046     _controls->setSwingUpK(k);
00047     printGains();
00048 }
00049 
00050 void Comm::setSwingUpD(Arguments* input, Reply* output){
00051     if (input->argc < 1){
00052         throwNotEnoughArgsError();
00053         return;
00054     }
00055     float d = input->getArg<float>(); 
00056     _controls->setSwingUpD(d);
00057     printGains();
00058 }
00059 
00060 
00061 void Comm::setTargetingK(Arguments* input, Reply* output){
00062     if (input->argc < 1){
00063         throwNotEnoughArgsError();
00064         return;
00065     }
00066     float k = input->getArg<float>(); 
00067     _controls->setTargetingK(k);
00068     printGains();
00069 }
00070 void Comm::setTargetingD(Arguments* input, Reply* output){
00071     if (input->argc < 1){
00072         throwNotEnoughArgsError();
00073         return;
00074     }
00075     float d = input->getArg<float>(); 
00076     _controls->setTargetingD(d);
00077     printGains();
00078 }
00079 
00080 
00081 void Comm::setDesiredThetaP(Arguments* input, Reply* output){
00082     if (input->argc < 1){
00083         throwNotEnoughArgsError();
00084         return;
00085     }
00086     float p = input->getArg<float>(); 
00087     _controls->setDesiredThetaP(p);
00088     printGains();
00089 }
00090 
00091 void Comm::setSoftLimitsP(Arguments* input, Reply* output){
00092     if (input->argc < 1){
00093         throwNotEnoughArgsError();
00094         return;
00095     }
00096     float p = input->getArg<float>(); 
00097     _controls->setSoftLimitsP(p);
00098     printGains();
00099 }
00100 
00101 
00102 
00103 void Comm::printGains(){
00104     _json.open();
00105     _json.print("swingUpK", _controls->getSwingUpK());
00106     _json.sepItem();
00107     _json.print("swingUpD", _controls->getSwingUpD());
00108     _json.sepItem();
00109     _json.print("desiredThetaP", _controls->getDesiredThetaP());
00110     _json.sepItem();
00111     _json.print("softLimitsP", _controls->getSoftLimitsP());
00112     _json.sepItem();
00113     _json.print("targetingK", _controls->getTargetingK());
00114     _json.sepItem();
00115     _json.print("targetingD", _controls->getTargetingD());
00116     _json.close();
00117 }
00118 
00119 void Comm::setTarget(Arguments* input, Reply* output){
00120     if (input->argc < 1){
00121         throwNotEnoughArgsError();
00122         return;
00123     }
00124     int targetPosition = input->getArg<int>(); 
00125     _controls->setTargetPosition(targetPosition);
00126     printTarget();
00127 }
00128 
00129 void Comm::printTarget(){
00130     _json.open();
00131     _json.print("target", _controls->getTargetPosition());
00132     _json.close();
00133 }
00134 
00135 void Comm::setTheta(Arguments * input, Reply * output){
00136     if (input->argc < 1){
00137         throwNotEnoughArgsError();
00138         return;
00139     }
00140     float theta = input->getArg<float>(); 
00141     _pc.printf("%f\n", theta);
00142     _controls->setTheta(theta);
00143 }
00144 
00145 void Comm::printPosition(){
00146     _json.open();
00147     _json.print("th1", _controls->getTheta1());
00148     _json.sepItem();
00149 //    _json.print("dth1", controls->getDTheta1());
00150 //    _json.sepItem();
00151     _json.print("th2", _controls->getTheta2());
00152 //    _json.sepItem();
00153 //    _json.print("dth2", _controls->getDTheta2());
00154     _json.close();
00155 }
00156 
00157 void Comm::printVelocity(){
00158         _json.open();
00159     _json.print("dth1", _controls->getDTheta1());
00160     _json.sepItem();
00161     _json.print("dth2", _controls->getDTheta2());
00162     _json.close();
00163 }
00164 
00165 void Comm::check(){
00166     if(!_pc.readable()) return;
00167     _pc.gets(buf, 256);
00168     //Call the static call method on the RPC class
00169     RPC::call(buf, outbuf); 
00170 //    _pc.printf("%s\n", outbuf);
00171 }
00172 
00173 Serial* Comm::getPC(){
00174     return &_pc;
00175 }
00176 
00177 void Comm::throwNotEnoughArgsError(){
00178     _json.open();
00179     _json.print("error", "not enough input arguments");
00180     _json.close();
00181 }