bio robot
Dependencies: MPU6050-DMP QEI_hw mbed-rpc mbed
Fork of MPU6050_Example by
Comm/Comm.cpp@17:8a0e647cf551, 2015-12-11 (annotated)
- Committer:
- amandaghassaei
- Date:
- Fri Dec 11 00:44:45 2015 +0000
- Revision:
- 17:8a0e647cf551
- Parent:
- 14:d620415259b1
- Child:
- 19:270735e44c98
eod
Who changed what in which revision?
User | Revision | Line number | New 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 | 2:17b18ea93551 | 91 | |
amandaghassaei | 2:17b18ea93551 | 92 | void Comm::printGains(){ |
amandaghassaei | 2:17b18ea93551 | 93 | _json.open(); |
amandaghassaei | 9:1d9b24d7ac77 | 94 | _json.print("swingUpK", _controls->getSwingUpK()); |
amandaghassaei | 2:17b18ea93551 | 95 | _json.sepItem(); |
amandaghassaei | 9:1d9b24d7ac77 | 96 | _json.print("swingUpD", _controls->getSwingUpD()); |
amandaghassaei | 2:17b18ea93551 | 97 | _json.sepItem(); |
amandaghassaei | 14:d620415259b1 | 98 | _json.print("desiredThetaP", _controls->getDesiredThetaP()); |
amandaghassaei | 17:8a0e647cf551 | 99 | _json.sepItem(); |
amandaghassaei | 17:8a0e647cf551 | 100 | _json.print("targetingK", _controls->getTargetingK()); |
amandaghassaei | 17:8a0e647cf551 | 101 | _json.sepItem(); |
amandaghassaei | 17:8a0e647cf551 | 102 | _json.print("targetingD", _controls->getTargetingD()); |
amandaghassaei | 2:17b18ea93551 | 103 | _json.close(); |
amandaghassaei | 2:17b18ea93551 | 104 | } |
amandaghassaei | 2:17b18ea93551 | 105 | |
amandaghassaei | 5:c3d671959ef3 | 106 | void Comm::setTarget(Arguments* input, Reply* output){ |
amandaghassaei | 5:c3d671959ef3 | 107 | if (input->argc < 1){ |
amandaghassaei | 5:c3d671959ef3 | 108 | throwNotEnoughArgsError(); |
amandaghassaei | 5:c3d671959ef3 | 109 | return; |
amandaghassaei | 5:c3d671959ef3 | 110 | } |
amandaghassaei | 5:c3d671959ef3 | 111 | int targetPosition = input->getArg<int>(); |
amandaghassaei | 9:1d9b24d7ac77 | 112 | _controls->setTargetPosition(targetPosition); |
amandaghassaei | 5:c3d671959ef3 | 113 | printTarget(); |
amandaghassaei | 2:17b18ea93551 | 114 | } |
amandaghassaei | 2:17b18ea93551 | 115 | |
amandaghassaei | 2:17b18ea93551 | 116 | void Comm::printTarget(){ |
amandaghassaei | 2:17b18ea93551 | 117 | _json.open(); |
amandaghassaei | 9:1d9b24d7ac77 | 118 | _json.print("target", _controls->getTargetPosition()); |
amandaghassaei | 2:17b18ea93551 | 119 | _json.close(); |
amandaghassaei | 2:17b18ea93551 | 120 | } |
amandaghassaei | 2:17b18ea93551 | 121 | |
amandaghassaei | 11:711d3c207e8c | 122 | void Comm::setTheta(Arguments * input, Reply * output){ |
amandaghassaei | 9:1d9b24d7ac77 | 123 | if (input->argc < 1){ |
amandaghassaei | 9:1d9b24d7ac77 | 124 | throwNotEnoughArgsError(); |
amandaghassaei | 9:1d9b24d7ac77 | 125 | return; |
amandaghassaei | 9:1d9b24d7ac77 | 126 | } |
amandaghassaei | 11:711d3c207e8c | 127 | float theta = input->getArg<float>(); |
amandaghassaei | 11:711d3c207e8c | 128 | _pc.printf("%f\n", theta); |
amandaghassaei | 11:711d3c207e8c | 129 | _controls->setTheta(theta); |
amandaghassaei | 9:1d9b24d7ac77 | 130 | } |
amandaghassaei | 9:1d9b24d7ac77 | 131 | |
amandaghassaei | 9:1d9b24d7ac77 | 132 | void Comm::printPosition(){ |
amandaghassaei | 2:17b18ea93551 | 133 | _json.open(); |
amandaghassaei | 9:1d9b24d7ac77 | 134 | _json.print("th1", _controls->getTheta1()); |
amandaghassaei | 9:1d9b24d7ac77 | 135 | _json.sepItem(); |
amandaghassaei | 9:1d9b24d7ac77 | 136 | // _json.print("dth1", controls->getDTheta1()); |
amandaghassaei | 8:1a3a69fecedf | 137 | // _json.sepItem(); |
amandaghassaei | 9:1d9b24d7ac77 | 138 | _json.print("th2", _controls->getTheta2()); |
amandaghassaei | 8:1a3a69fecedf | 139 | // _json.sepItem(); |
amandaghassaei | 10:769cc457c3a4 | 140 | // _json.print("dth2", _controls->getDTheta2()); |
amandaghassaei | 2:17b18ea93551 | 141 | _json.close(); |
amandaghassaei | 2:17b18ea93551 | 142 | } |
amandaghassaei | 2:17b18ea93551 | 143 | |
amandaghassaei | 11:711d3c207e8c | 144 | void Comm::printVelocity(){ |
amandaghassaei | 11:711d3c207e8c | 145 | _json.open(); |
amandaghassaei | 11:711d3c207e8c | 146 | _json.print("dth1", _controls->getDTheta1()); |
amandaghassaei | 11:711d3c207e8c | 147 | _json.sepItem(); |
amandaghassaei | 11:711d3c207e8c | 148 | _json.print("dth2", _controls->getDTheta2()); |
amandaghassaei | 11:711d3c207e8c | 149 | _json.close(); |
amandaghassaei | 11:711d3c207e8c | 150 | } |
amandaghassaei | 11:711d3c207e8c | 151 | |
amandaghassaei | 4:ebb882658c50 | 152 | void Comm::check(){ |
amandaghassaei | 5:c3d671959ef3 | 153 | if(!_pc.readable()) return; |
amandaghassaei | 4:ebb882658c50 | 154 | _pc.gets(buf, 256); |
amandaghassaei | 4:ebb882658c50 | 155 | //Call the static call method on the RPC class |
amandaghassaei | 4:ebb882658c50 | 156 | RPC::call(buf, outbuf); |
amandaghassaei | 5:c3d671959ef3 | 157 | // _pc.printf("%s\n", outbuf); |
amandaghassaei | 4:ebb882658c50 | 158 | } |
amandaghassaei | 5:c3d671959ef3 | 159 | |
amandaghassaei | 9:1d9b24d7ac77 | 160 | Serial* Comm::getPC(){ |
amandaghassaei | 9:1d9b24d7ac77 | 161 | return &_pc; |
amandaghassaei | 9:1d9b24d7ac77 | 162 | } |
amandaghassaei | 9:1d9b24d7ac77 | 163 | |
amandaghassaei | 5:c3d671959ef3 | 164 | void Comm::throwNotEnoughArgsError(){ |
amandaghassaei | 5:c3d671959ef3 | 165 | _json.open(); |
amandaghassaei | 5:c3d671959ef3 | 166 | _json.print("error", "not enough input arguments"); |
amandaghassaei | 5:c3d671959ef3 | 167 | _json.close(); |
amandaghassaei | 5:c3d671959ef3 | 168 | } |