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@8:1a3a69fecedf, 2015-12-03 (annotated)

Committer:
amandaghassaei
Date:
Thu Dec 03 23:55:44 2015 +0000
Revision:
8:1a3a69fecedf
Parent:
6:12b6be801ad8
Child:
9:1d9b24d7ac77
added in matlab controls;

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 8:1a3a69fecedf 4 Comm::Comm(CommDelegate *gains, CommDelegate *target):_pc(USBTX, USBRX), _json(&_pc)
amandaghassaei 2:17b18ea93551 5 {
amandaghassaei 6:12b6be801ad8 6 _pc.baud(115200);
amandaghassaei 2:17b18ea93551 7 _gains = gains;
amandaghassaei 2:17b18ea93551 8 _target = target;
amandaghassaei 2:17b18ea93551 9 }
amandaghassaei 2:17b18ea93551 10
amandaghassaei 5:c3d671959ef3 11 void Comm::openGripper1(Arguments* input, Reply* output){
amandaghassaei 2:17b18ea93551 12 printGripper1State(true);
amandaghassaei 2:17b18ea93551 13 }
amandaghassaei 2:17b18ea93551 14
amandaghassaei 5:c3d671959ef3 15 void Comm::closeGripper1(Arguments* input, Reply* output){
amandaghassaei 2:17b18ea93551 16 printGripper1State(false);
amandaghassaei 2:17b18ea93551 17 }
amandaghassaei 2:17b18ea93551 18
amandaghassaei 2:17b18ea93551 19 void Comm::printGripper1State(bool state){
amandaghassaei 2:17b18ea93551 20 _json.open();
amandaghassaei 2:17b18ea93551 21 if (state) _json.print("gripper1", "true");
amandaghassaei 2:17b18ea93551 22 else _json.print("gripper1", "false");
amandaghassaei 2:17b18ea93551 23 _json.close();
amandaghassaei 2:17b18ea93551 24 }
amandaghassaei 2:17b18ea93551 25
amandaghassaei 5:c3d671959ef3 26 void Comm::openGripper2(Arguments* input, Reply* output){
amandaghassaei 2:17b18ea93551 27 printGripper2State(true);
amandaghassaei 2:17b18ea93551 28 }
amandaghassaei 2:17b18ea93551 29
amandaghassaei 5:c3d671959ef3 30 void Comm::closeGripper2(Arguments* input, Reply* output){
amandaghassaei 2:17b18ea93551 31 printGripper2State(false);
amandaghassaei 2:17b18ea93551 32 }
amandaghassaei 2:17b18ea93551 33
amandaghassaei 2:17b18ea93551 34 void Comm::printGripper2State(bool state){
amandaghassaei 2:17b18ea93551 35 _json.open();
amandaghassaei 2:17b18ea93551 36 if (state) _json.print("gripper2", "true");
amandaghassaei 2:17b18ea93551 37 else _json.print("gripper2", "false");
amandaghassaei 2:17b18ea93551 38 _json.close();
amandaghassaei 2:17b18ea93551 39 }
amandaghassaei 2:17b18ea93551 40
amandaghassaei 5:c3d671959ef3 41 void Comm::setGains(Arguments* input, Reply* output){
amandaghassaei 5:c3d671959ef3 42 if (input->argc < _gains->numGains()){
amandaghassaei 5:c3d671959ef3 43 throwNotEnoughArgsError();
amandaghassaei 5:c3d671959ef3 44 return;
amandaghassaei 5:c3d671959ef3 45 }
amandaghassaei 5:c3d671959ef3 46 float k1 = input->getArg<float>();
amandaghassaei 5:c3d671959ef3 47 float d1 = input->getArg<float>();
amandaghassaei 5:c3d671959ef3 48 float k2 = input->getArg<float>();
amandaghassaei 5:c3d671959ef3 49 float d2 = input->getArg<float>();
amandaghassaei 2:17b18ea93551 50 _gains->setGains(k1, d1, k2, d2);
amandaghassaei 5:c3d671959ef3 51 printGains();
amandaghassaei 2:17b18ea93551 52 }
amandaghassaei 2:17b18ea93551 53
amandaghassaei 2:17b18ea93551 54 void Comm::printGains(){
amandaghassaei 2:17b18ea93551 55 _json.open();
amandaghassaei 2:17b18ea93551 56 _json.print("k1", _gains->getK1());
amandaghassaei 2:17b18ea93551 57 _json.sepItem();
amandaghassaei 2:17b18ea93551 58 _json.print("d1", _gains->getD1());
amandaghassaei 2:17b18ea93551 59 _json.sepItem();
amandaghassaei 2:17b18ea93551 60 _json.print("k2", _gains->getK2());
amandaghassaei 2:17b18ea93551 61 _json.sepItem();
amandaghassaei 2:17b18ea93551 62 _json.print("d2", _gains->getD1());
amandaghassaei 2:17b18ea93551 63 _json.close();
amandaghassaei 2:17b18ea93551 64 }
amandaghassaei 2:17b18ea93551 65
amandaghassaei 5:c3d671959ef3 66 void Comm::setTarget(Arguments* input, Reply* output){
amandaghassaei 5:c3d671959ef3 67 if (input->argc < 1){
amandaghassaei 5:c3d671959ef3 68 throwNotEnoughArgsError();
amandaghassaei 5:c3d671959ef3 69 return;
amandaghassaei 5:c3d671959ef3 70 }
amandaghassaei 5:c3d671959ef3 71 int targetPosition = input->getArg<int>();
amandaghassaei 2:17b18ea93551 72 _target->setPosition(targetPosition);
amandaghassaei 5:c3d671959ef3 73 printTarget();
amandaghassaei 2:17b18ea93551 74 }
amandaghassaei 2:17b18ea93551 75
amandaghassaei 2:17b18ea93551 76 void Comm::printTarget(){
amandaghassaei 2:17b18ea93551 77 _json.open();
amandaghassaei 2:17b18ea93551 78 _json.print("target", _target->getPosition());
amandaghassaei 2:17b18ea93551 79 _json.close();
amandaghassaei 2:17b18ea93551 80 }
amandaghassaei 2:17b18ea93551 81
amandaghassaei 8:1a3a69fecedf 82 void Comm::printPosition(CommDelegate *controls){
amandaghassaei 2:17b18ea93551 83 _json.open();
amandaghassaei 8:1a3a69fecedf 84 _json.print("th1", controls->getTheta1());
amandaghassaei 8:1a3a69fecedf 85 // _json.sepItem();
amandaghassaei 8:1a3a69fecedf 86 // _json.print("dth1", controls->getDTheta1());
amandaghassaei 2:17b18ea93551 87 _json.sepItem();
amandaghassaei 8:1a3a69fecedf 88 _json.print("th2", controls->getTheta2());
amandaghassaei 8:1a3a69fecedf 89 // _json.sepItem();
amandaghassaei 8:1a3a69fecedf 90 // _json.print("dth2", controls->getDTheta2());
amandaghassaei 2:17b18ea93551 91 _json.close();
amandaghassaei 2:17b18ea93551 92 }
amandaghassaei 2:17b18ea93551 93
amandaghassaei 4:ebb882658c50 94 void Comm::check(){
amandaghassaei 5:c3d671959ef3 95 if(!_pc.readable()) return;
amandaghassaei 4:ebb882658c50 96 _pc.gets(buf, 256);
amandaghassaei 4:ebb882658c50 97 //Call the static call method on the RPC class
amandaghassaei 4:ebb882658c50 98 RPC::call(buf, outbuf);
amandaghassaei 5:c3d671959ef3 99 // _pc.printf("%s\n", outbuf);
amandaghassaei 4:ebb882658c50 100 }
amandaghassaei 5:c3d671959ef3 101
amandaghassaei 5:c3d671959ef3 102 void Comm::throwNotEnoughArgsError(){
amandaghassaei 5:c3d671959ef3 103 _json.open();
amandaghassaei 5:c3d671959ef3 104 _json.print("error", "not enough input arguments");
amandaghassaei 5:c3d671959ef3 105 _json.close();
amandaghassaei 5:c3d671959ef3 106 }