2022_gorobo_Ateam / gourobo2022_undercarriage_new

2022_Ateam_MOTORprogramをscrp_slaveでメインマイコンからコントローラー状況を読み取れるように改良。 また、モータに0以外のpwmが送られている場合に基盤付属のledが点灯するようにした。

Dependencies:   SBDBT arrc_mbed BNO055

TARGETco.cpp

Committer:
guesta
Date:
2022-04-08
Revision:
13:369f4abc1f36
Parent:
7:a0375e6dc8ca

File content as of revision 13:369f4abc1f36:

#include "TARGETco.hpp"

void TARGETco::pass_val(double XPOWER,double YPOWER,double THETA){
    Xpower = XPOWER;
    Ypower = YPOWER;
    theta = THETA;
    
    cal_angular();
}

void TARGETco::pass_target(double TARGET){
    for(int i = 0;i < 4;i ++){
        target[i] = TARGET;
    }
}

void TARGETco::cal_angular(){
    Xvelocity = Xpower * cos(theta) - Ypower * sin(theta);
    Yvelocity = -Xpower * sin(theta) - Ypower * cos(theta);
    target[0] = -Xvelocity;
    target[1] = -Yvelocity;
    target[2] = Xvelocity;
    target[3] = Yvelocity;
    //printf("%lf\n",target[1]);
}

double TARGETco::obt_target1(){ return target[0]; }

double TARGETco::obt_target2(){ return target[1]; }

double TARGETco::obt_target3(){ return target[2]; }

double TARGETco::obt_target4(){ return target[3]; }