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@9:1d9b24d7ac77, 2015-12-05 (annotated)

Committer:
amandaghassaei
Date:
Sat Dec 05 00:40:42 2015 +0000
Revision:
9:1d9b24d7ac77
Parent:
8:1a3a69fecedf
Child:
10:769cc457c3a4
basic motor pwm working

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 6:12b6be801ad8 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 2:17b18ea93551 59
amandaghassaei 2:17b18ea93551 60 void Comm::printGains(){
amandaghassaei 2:17b18ea93551 61 _json.open();
amandaghassaei 9:1d9b24d7ac77 62 _json.print("swingUpK", _controls->getSwingUpK());
amandaghassaei 2:17b18ea93551 63 _json.sepItem();
amandaghassaei 9:1d9b24d7ac77 64 _json.print("swingUpD", _controls->getSwingUpD());
amandaghassaei 2:17b18ea93551 65 _json.sepItem();
amandaghassaei 9:1d9b24d7ac77 66 _json.print("k2", _controls->getK2());
amandaghassaei 2:17b18ea93551 67 _json.sepItem();
amandaghassaei 9:1d9b24d7ac77 68 _json.print("d2", _controls->getD2());
amandaghassaei 2:17b18ea93551 69 _json.close();
amandaghassaei 2:17b18ea93551 70 }
amandaghassaei 2:17b18ea93551 71
amandaghassaei 5:c3d671959ef3 72 void Comm::setTarget(Arguments* input, Reply* output){
amandaghassaei 5:c3d671959ef3 73 if (input->argc < 1){
amandaghassaei 5:c3d671959ef3 74 throwNotEnoughArgsError();
amandaghassaei 5:c3d671959ef3 75 return;
amandaghassaei 5:c3d671959ef3 76 }
amandaghassaei 5:c3d671959ef3 77 int targetPosition = input->getArg<int>();
amandaghassaei 9:1d9b24d7ac77 78 _controls->setTargetPosition(targetPosition);
amandaghassaei 5:c3d671959ef3 79 printTarget();
amandaghassaei 2:17b18ea93551 80 }
amandaghassaei 2:17b18ea93551 81
amandaghassaei 2:17b18ea93551 82 void Comm::printTarget(){
amandaghassaei 2:17b18ea93551 83 _json.open();
amandaghassaei 9:1d9b24d7ac77 84 _json.print("target", _controls->getTargetPosition());
amandaghassaei 2:17b18ea93551 85 _json.close();
amandaghassaei 2:17b18ea93551 86 }
amandaghassaei 2:17b18ea93551 87
amandaghassaei 9:1d9b24d7ac77 88 void Comm::setTorque(Arguments * input, Reply * output){
amandaghassaei 9:1d9b24d7ac77 89 if (input->argc < 1){
amandaghassaei 9:1d9b24d7ac77 90 throwNotEnoughArgsError();
amandaghassaei 9:1d9b24d7ac77 91 return;
amandaghassaei 9:1d9b24d7ac77 92 }
amandaghassaei 9:1d9b24d7ac77 93 float torque = atof(input->argv[0]);
amandaghassaei 9:1d9b24d7ac77 94 // _pc.printf("%f", torque);
amandaghassaei 9:1d9b24d7ac77 95 _controls->setTorque(torque);
amandaghassaei 9:1d9b24d7ac77 96 }
amandaghassaei 9:1d9b24d7ac77 97
amandaghassaei 9:1d9b24d7ac77 98 void Comm::printPosition(){
amandaghassaei 2:17b18ea93551 99 _json.open();
amandaghassaei 9:1d9b24d7ac77 100 _json.print("th1", _controls->getTheta1());
amandaghassaei 9:1d9b24d7ac77 101 _json.sepItem();
amandaghassaei 9:1d9b24d7ac77 102 // _json.print("dth1", controls->getDTheta1());
amandaghassaei 8:1a3a69fecedf 103 // _json.sepItem();
amandaghassaei 9:1d9b24d7ac77 104 _json.print("th2", _controls->getTheta2());
amandaghassaei 8:1a3a69fecedf 105 // _json.sepItem();
amandaghassaei 8:1a3a69fecedf 106 // _json.print("dth2", controls->getDTheta2());
amandaghassaei 2:17b18ea93551 107 _json.close();
amandaghassaei 2:17b18ea93551 108 }
amandaghassaei 2:17b18ea93551 109
amandaghassaei 4:ebb882658c50 110 void Comm::check(){
amandaghassaei 5:c3d671959ef3 111 if(!_pc.readable()) return;
amandaghassaei 4:ebb882658c50 112 _pc.gets(buf, 256);
amandaghassaei 4:ebb882658c50 113 //Call the static call method on the RPC class
amandaghassaei 4:ebb882658c50 114 RPC::call(buf, outbuf);
amandaghassaei 5:c3d671959ef3 115 // _pc.printf("%s\n", outbuf);
amandaghassaei 4:ebb882658c50 116 }
amandaghassaei 5:c3d671959ef3 117
amandaghassaei 9:1d9b24d7ac77 118 Serial* Comm::getPC(){
amandaghassaei 9:1d9b24d7ac77 119 return &_pc;
amandaghassaei 9:1d9b24d7ac77 120 }
amandaghassaei 9:1d9b24d7ac77 121
amandaghassaei 5:c3d671959ef3 122 void Comm::throwNotEnoughArgsError(){
amandaghassaei 5:c3d671959ef3 123 _json.open();
amandaghassaei 5:c3d671959ef3 124 _json.print("error", "not enough input arguments");
amandaghassaei 5:c3d671959ef3 125 _json.close();
amandaghassaei 5:c3d671959ef3 126 }