NagaokaRoboticsClub_mbedTeam / Mbed 2 deprecated NHK2017_octopus

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:   2017NHKpin_config mbed FEP ikarashiMDC PID jy901 omni HMC6352 omni_wheel

Fork of KANI2017v2 by NagaokaRoboticsClub_mbedTeam

bot/controller/controller.cpp

Committer:
UCHITAKE
Date:
2017-08-22
Revision:
2:ea151e05033a
Parent:
1:269914e0aa07
Child:
5:bc5ba4f070ad

File content as of revision 2:ea151e05033a:

#include "controller.h"

Controller::Controller() : FEP(controllerTX, controllerRX, ADDR)
{
    for(int i = 0; i < 7; i++) Button1[i] = 0;
    for(int i = 0; i < 6; i++) Button2[i] = 0;
    for(int i = 0; i < 6; i++) data[i] = 0;
    for(int i = 0; i < 4; i++) stick[i] = 0;
    for(int i = 0; i < 2; i++) degree[i] = 0;
    for(int i = 0; i < 2; i++) norm[i] = 0;
    for(int i = 0; i < 2; i++) vector[i] = 0;
    fepTemp = 0;
}

void Controller::receiveState()
{
    fepTemp = read(data, 6);
    if(fepTemp == FEP_SUCCESS) {
        for(int i=0; i<7; i++) {
            Button1[i] = data[4] % 2;
            data[4] /= 2;
        }
        for(int i=0; i<6; i++) {
            Button2[i] = data[5] % 2;
            data[5] /= 2;
        }
        for(int i = 0; i < 4; i++) {
            stick[i] = -((double)(data[i] / STICK_DIVIDE) * 2.0 - 1.0);
        }
        getStickData();
    } else if(fepTemp == FEP_NO_RESPONSE) {
    } else {
    }
}

void Controller::getStickData()
{
    degree[0] = (atan2(stick[1], -stick[0])) * (180 / M_PI);
    degree[1] = (atan2(stick[3], -stick[2])) * (180 / M_PI);

    norm[0] = hypot(stick[0], stick[1]);
    norm[1] = hypot(stick[2], stick[3]);

    if(norm[0] < STICK_NEWTRAL) norm[0] = 0;
    if(norm[1] < STICK_NEWTRAL) norm[1] = 0;
    if(norm[0] > STICK_NORM_MAX) norm[0] = STICK_NORM_MAX;
    if(norm[1] > STICK_NORM_MAX) norm[1] = STICK_NORM_MAX;

    vector[0] = degree[0];
    vector[1] = norm[0];
    vector2[0] = degree[1];
    vector2[1] = norm[1];
}