Amanda Ghassaei / Mbed 2 deprecated Bracky-MPU6050-DMP

bio robot

Dependencies:   MPU6050-DMP QEI_hw mbed-rpc mbed

Fork of MPU6050_Example by Shundo Kishi

Comm/Comm.cpp@20:f13b949b623b, 2015-12-11 (annotated)

Committer:
amandaghassaei
Date:
Fri Dec 11 08:54:32 2015 +0000
Revision:
20:f13b949b623b
Parent:
19:270735e44c98 
publish

Who changed what in which revision?

UserRevisionLine numberNew contents of line
amandaghassaei 2:17b18ea93551 1 #include "Comm.h"
amandaghassaei 2:17b18ea93551 2
amandaghassaei 2:17b18ea93551 3
amandaghassaei 9:1d9b24d7ac77 4 Comm::Comm(CommDelegate *controls):_pc(USBTX, USBRX), _json(&_pc)
amandaghassaei 2:17b18ea93551 5 {
amandaghassaei 11:711d3c207e8c 6 _pc.baud(115200);
amandaghassaei 9:1d9b24d7ac77 7 _controls = controls;
amandaghassaei 2:17b18ea93551 8 }
amandaghassaei 2:17b18ea93551 9
amandaghassaei 5:c3d671959ef3 10 void Comm::openGripper1(Arguments* input, Reply* output){
amandaghassaei 2:17b18ea93551 11 printGripper1State(true);
amandaghassaei 2:17b18ea93551 12 }
amandaghassaei 2:17b18ea93551 13
amandaghassaei 5:c3d671959ef3 14 void Comm::closeGripper1(Arguments* input, Reply* output){
amandaghassaei 2:17b18ea93551 15 printGripper1State(false);
amandaghassaei 2:17b18ea93551 16 }
amandaghassaei 2:17b18ea93551 17
amandaghassaei 2:17b18ea93551 18 void Comm::printGripper1State(bool state){
amandaghassaei 2:17b18ea93551 19 _json.open();
amandaghassaei 2:17b18ea93551 20 if (state) _json.print("gripper1", "true");
amandaghassaei 2:17b18ea93551 21 else _json.print("gripper1", "false");
amandaghassaei 2:17b18ea93551 22 _json.close();
amandaghassaei 2:17b18ea93551 23 }
amandaghassaei 2:17b18ea93551 24
amandaghassaei 5:c3d671959ef3 25 void Comm::openGripper2(Arguments* input, Reply* output){
amandaghassaei 2:17b18ea93551 26 printGripper2State(true);
amandaghassaei 2:17b18ea93551 27 }
amandaghassaei 2:17b18ea93551 28
amandaghassaei 5:c3d671959ef3 29 void Comm::closeGripper2(Arguments* input, Reply* output){
amandaghassaei 2:17b18ea93551 30 printGripper2State(false);
amandaghassaei 2:17b18ea93551 31 }
amandaghassaei 2:17b18ea93551 32
amandaghassaei 2:17b18ea93551 33 void Comm::printGripper2State(bool state){
amandaghassaei 2:17b18ea93551 34 _json.open();
amandaghassaei 2:17b18ea93551 35 if (state) _json.print("gripper2", "true");
amandaghassaei 2:17b18ea93551 36 else _json.print("gripper2", "false");
amandaghassaei 2:17b18ea93551 37 _json.close();
amandaghassaei 2:17b18ea93551 38 }
amandaghassaei 2:17b18ea93551 39
amandaghassaei 9:1d9b24d7ac77 40 void Comm::setSwingUpK(Arguments* input, Reply* output){
amandaghassaei 9:1d9b24d7ac77 41 if (input->argc < 1){
amandaghassaei 5:c3d671959ef3 42 throwNotEnoughArgsError();
amandaghassaei 5:c3d671959ef3 43 return;
amandaghassaei 5:c3d671959ef3 44 }
amandaghassaei 9:1d9b24d7ac77 45 float k = input->getArg<float>();
amandaghassaei 9:1d9b24d7ac77 46 _controls->setSwingUpK(k);
amandaghassaei 9:1d9b24d7ac77 47 printGains();
amandaghassaei 9:1d9b24d7ac77 48 }
amandaghassaei 9:1d9b24d7ac77 49
amandaghassaei 9:1d9b24d7ac77 50 void Comm::setSwingUpD(Arguments* input, Reply* output){
amandaghassaei 9:1d9b24d7ac77 51 if (input->argc < 1){
amandaghassaei 9:1d9b24d7ac77 52 throwNotEnoughArgsError();
amandaghassaei 9:1d9b24d7ac77 53 return;
amandaghassaei 9:1d9b24d7ac77 54 }
amandaghassaei 9:1d9b24d7ac77 55 float d = input->getArg<float>();
amandaghassaei 9:1d9b24d7ac77 56 _controls->setSwingUpD(d);
amandaghassaei 5:c3d671959ef3 57 printGains();
amandaghassaei 2:17b18ea93551 58 }
amandaghassaei 17:8a0e647cf551 59
amandaghassaei 17:8a0e647cf551 60
amandaghassaei 17:8a0e647cf551 61 void Comm::setTargetingK(Arguments* input, Reply* output){
amandaghassaei 17:8a0e647cf551 62 if (input->argc < 1){
amandaghassaei 17:8a0e647cf551 63 throwNotEnoughArgsError();
amandaghassaei 17:8a0e647cf551 64 return;
amandaghassaei 17:8a0e647cf551 65 }
amandaghassaei 17:8a0e647cf551 66 float k = input->getArg<float>();
amandaghassaei 17:8a0e647cf551 67 _controls->setTargetingK(k);
amandaghassaei 17:8a0e647cf551 68 printGains();
amandaghassaei 17:8a0e647cf551 69 }
amandaghassaei 17:8a0e647cf551 70 void Comm::setTargetingD(Arguments* input, Reply* output){
amandaghassaei 17:8a0e647cf551 71 if (input->argc < 1){
amandaghassaei 17:8a0e647cf551 72 throwNotEnoughArgsError();
amandaghassaei 17:8a0e647cf551 73 return;
amandaghassaei 17:8a0e647cf551 74 }
amandaghassaei 17:8a0e647cf551 75 float d = input->getArg<float>();
amandaghassaei 17:8a0e647cf551 76 _controls->setTargetingD(d);
amandaghassaei 17:8a0e647cf551 77 printGains();
amandaghassaei 17:8a0e647cf551 78 }
amandaghassaei 17:8a0e647cf551 79
amandaghassaei 17:8a0e647cf551 80
amandaghassaei 14:d620415259b1 81 void Comm::setDesiredThetaP(Arguments* input, Reply* output){
amandaghassaei 10:769cc457c3a4 82 if (input->argc < 1){
amandaghassaei 10:769cc457c3a4 83 throwNotEnoughArgsError();
amandaghassaei 10:769cc457c3a4 84 return;
amandaghassaei 10:769cc457c3a4 85 }
amandaghassaei 10:769cc457c3a4 86 float p = input->getArg<float>();
amandaghassaei 14:d620415259b1 87 _controls->setDesiredThetaP(p);
amandaghassaei 10:769cc457c3a4 88 printGains();
amandaghassaei 10:769cc457c3a4 89 }
amandaghassaei 10:769cc457c3a4 90
amandaghassaei 19:270735e44c98 91 void Comm::setSoftLimitsP(Arguments* input, Reply* output){
amandaghassaei 19:270735e44c98 92 if (input->argc < 1){
amandaghassaei 19:270735e44c98 93 throwNotEnoughArgsError();
amandaghassaei 19:270735e44c98 94 return;
amandaghassaei 19:270735e44c98 95 }
amandaghassaei 19:270735e44c98 96 float p = input->getArg<float>();
amandaghassaei 19:270735e44c98 97 _controls->setSoftLimitsP(p);
amandaghassaei 19:270735e44c98 98 printGains();
amandaghassaei 19:270735e44c98 99 }
amandaghassaei 19:270735e44c98 100
amandaghassaei 19:270735e44c98 101
amandaghassaei 2:17b18ea93551 102
amandaghassaei 2:17b18ea93551 103 void Comm::printGains(){
amandaghassaei 2:17b18ea93551 104 _json.open();
amandaghassaei 9:1d9b24d7ac77 105 _json.print("swingUpK", _controls->getSwingUpK());
amandaghassaei 2:17b18ea93551 106 _json.sepItem();
amandaghassaei 9:1d9b24d7ac77 107 _json.print("swingUpD", _controls->getSwingUpD());
amandaghassaei 2:17b18ea93551 108 _json.sepItem();
amandaghassaei 14:d620415259b1 109 _json.print("desiredThetaP", _controls->getDesiredThetaP());
amandaghassaei 17:8a0e647cf551 110 _json.sepItem();
amandaghassaei 19:270735e44c98 111 _json.print("softLimitsP", _controls->getSoftLimitsP());
amandaghassaei 19:270735e44c98 112 _json.sepItem();
amandaghassaei 17:8a0e647cf551 113 _json.print("targetingK", _controls->getTargetingK());
amandaghassaei 17:8a0e647cf551 114 _json.sepItem();
amandaghassaei 17:8a0e647cf551 115 _json.print("targetingD", _controls->getTargetingD());
amandaghassaei 2:17b18ea93551 116 _json.close();
amandaghassaei 2:17b18ea93551 117 }
amandaghassaei 2:17b18ea93551 118
amandaghassaei 5:c3d671959ef3 119 void Comm::setTarget(Arguments* input, Reply* output){
amandaghassaei 5:c3d671959ef3 120 if (input->argc < 1){
amandaghassaei 5:c3d671959ef3 121 throwNotEnoughArgsError();
amandaghassaei 5:c3d671959ef3 122 return;
amandaghassaei 5:c3d671959ef3 123 }
amandaghassaei 5:c3d671959ef3 124 int targetPosition = input->getArg<int>();
amandaghassaei 9:1d9b24d7ac77 125 _controls->setTargetPosition(targetPosition);
amandaghassaei 5:c3d671959ef3 126 printTarget();
amandaghassaei 2:17b18ea93551 127 }
amandaghassaei 2:17b18ea93551 128
amandaghassaei 2:17b18ea93551 129 void Comm::printTarget(){
amandaghassaei 2:17b18ea93551 130 _json.open();
amandaghassaei 9:1d9b24d7ac77 131 _json.print("target", _controls->getTargetPosition());
amandaghassaei 2:17b18ea93551 132 _json.close();
amandaghassaei 2:17b18ea93551 133 }
amandaghassaei 2:17b18ea93551 134
amandaghassaei 11:711d3c207e8c 135 void Comm::setTheta(Arguments * input, Reply * output){
amandaghassaei 9:1d9b24d7ac77 136 if (input->argc < 1){
amandaghassaei 9:1d9b24d7ac77 137 throwNotEnoughArgsError();
amandaghassaei 9:1d9b24d7ac77 138 return;
amandaghassaei 9:1d9b24d7ac77 139 }
amandaghassaei 11:711d3c207e8c 140 float theta = input->getArg<float>();
amandaghassaei 11:711d3c207e8c 141 _pc.printf("%f\n", theta);
amandaghassaei 11:711d3c207e8c 142 _controls->setTheta(theta);
amandaghassaei 9:1d9b24d7ac77 143 }
amandaghassaei 9:1d9b24d7ac77 144
amandaghassaei 9:1d9b24d7ac77 145 void Comm::printPosition(){
amandaghassaei 2:17b18ea93551 146 _json.open();
amandaghassaei 9:1d9b24d7ac77 147 _json.print("th1", _controls->getTheta1());
amandaghassaei 9:1d9b24d7ac77 148 _json.sepItem();
amandaghassaei 9:1d9b24d7ac77 149 // _json.print("dth1", controls->getDTheta1());
amandaghassaei 8:1a3a69fecedf 150 // _json.sepItem();
amandaghassaei 9:1d9b24d7ac77 151 _json.print("th2", _controls->getTheta2());
amandaghassaei 8:1a3a69fecedf 152 // _json.sepItem();
amandaghassaei 10:769cc457c3a4 153 // _json.print("dth2", _controls->getDTheta2());
amandaghassaei 2:17b18ea93551 154 _json.close();
amandaghassaei 2:17b18ea93551 155 }
amandaghassaei 2:17b18ea93551 156
amandaghassaei 11:711d3c207e8c 157 void Comm::printVelocity(){
amandaghassaei 11:711d3c207e8c 158 _json.open();
amandaghassaei 11:711d3c207e8c 159 _json.print("dth1", _controls->getDTheta1());
amandaghassaei 11:711d3c207e8c 160 _json.sepItem();
amandaghassaei 11:711d3c207e8c 161 _json.print("dth2", _controls->getDTheta2());
amandaghassaei 11:711d3c207e8c 162 _json.close();
amandaghassaei 11:711d3c207e8c 163 }
amandaghassaei 11:711d3c207e8c 164
amandaghassaei 4:ebb882658c50 165 void Comm::check(){
amandaghassaei 5:c3d671959ef3 166 if(!_pc.readable()) return;
amandaghassaei 4:ebb882658c50 167 _pc.gets(buf, 256);
amandaghassaei 4:ebb882658c50 168 //Call the static call method on the RPC class
amandaghassaei 4:ebb882658c50 169 RPC::call(buf, outbuf);
amandaghassaei 5:c3d671959ef3 170 // _pc.printf("%s\n", outbuf);
amandaghassaei 4:ebb882658c50 171 }
amandaghassaei 5:c3d671959ef3 172
amandaghassaei 9:1d9b24d7ac77 173 Serial* Comm::getPC(){
amandaghassaei 9:1d9b24d7ac77 174 return &_pc;
amandaghassaei 9:1d9b24d7ac77 175 }
amandaghassaei 9:1d9b24d7ac77 176
amandaghassaei 5:c3d671959ef3 177 void Comm::throwNotEnoughArgsError(){
amandaghassaei 5:c3d671959ef3 178 _json.open();
amandaghassaei 5:c3d671959ef3 179 _json.print("error", "not enough input arguments");
amandaghassaei 5:c3d671959ef3 180 _json.close();
amandaghassaei 5:c3d671959ef3 181 }