Amanda Ghassaei / Mbed 2 deprecated Bracky-MPU6050-DMP

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   MPU6050-DMP QEI_hw mbed-rpc mbed

Fork of MPU6050_Example by Shundo Kishi

Comm/Comm.cpp

Committer:
amandaghassaei
Date:
2015-12-03
Revision:
8:1a3a69fecedf
Parent:
6:12b6be801ad8
Child:
9:1d9b24d7ac77

File content as of revision 8:1a3a69fecedf:

#include "Comm.h"

    
Comm::Comm(CommDelegate *gains, CommDelegate *target):_pc(USBTX, USBRX), _json(&_pc)
{
    _pc.baud(115200);
    _gains = gains;
    _target = target;
}

void Comm::openGripper1(Arguments* input, Reply* output){
    printGripper1State(true);
}

void Comm::closeGripper1(Arguments* input, Reply* output){
    printGripper1State(false);
}

void Comm::printGripper1State(bool state){
    _json.open();
    if (state) _json.print("gripper1", "true");
    else _json.print("gripper1", "false");
    _json.close();
}

void Comm::openGripper2(Arguments* input, Reply* output){
    printGripper2State(true);
}

void Comm::closeGripper2(Arguments* input, Reply* output){
    printGripper2State(false);
}

void Comm::printGripper2State(bool state){
    _json.open();
    if (state) _json.print("gripper2", "true");
    else _json.print("gripper2", "false");
    _json.close();
}

void Comm::setGains(Arguments* input, Reply* output){
    if (input->argc < _gains->numGains()){
        throwNotEnoughArgsError();
        return;
    }
    float k1 = input->getArg<float>(); 
    float d1 = input->getArg<float>(); 
    float k2 = input->getArg<float>();  
    float d2 = input->getArg<float>(); 
    _gains->setGains(k1, d1, k2, d2);
    printGains();
}

void Comm::printGains(){
    _json.open();
    _json.print("k1", _gains->getK1());
    _json.sepItem();
    _json.print("d1", _gains->getD1());
    _json.sepItem();
    _json.print("k2", _gains->getK2());
    _json.sepItem();
    _json.print("d2", _gains->getD1());
    _json.close();
}

void Comm::setTarget(Arguments* input, Reply* output){
    if (input->argc < 1){
        throwNotEnoughArgsError();
        return;
    }
    int targetPosition = input->getArg<int>(); 
    _target->setPosition(targetPosition);
    printTarget();
}

void Comm::printTarget(){
    _json.open();
    _json.print("target", _target->getPosition());
    _json.close();
}

void Comm::printPosition(CommDelegate *controls){
    _json.open();
    _json.print("th1", controls->getTheta1());
//    _json.sepItem();
//    _json.print("dth1", controls->getDTheta1());
    _json.sepItem();
    _json.print("th2", controls->getTheta2());
//    _json.sepItem();
//    _json.print("dth2", controls->getDTheta2());
    _json.close();
}

void Comm::check(){
    if(!_pc.readable()) return;
    _pc.gets(buf, 256);
    //Call the static call method on the RPC class
    RPC::call(buf, outbuf); 
//    _pc.printf("%s\n", outbuf);
}

void Comm::throwNotEnoughArgsError(){
    _json.open();
    _json.print("error", "not enough input arguments");
    _json.close();
}