toshi mura
/
NRP2020_main
gjyrjyykkudt
main.cpp
- Committer:
- muratoshi
- Date:
- 2020-02-01
- Revision:
- 6:cd2671c141cc
- Parent:
- 5:8a0663298e2c
File content as of revision 6:cd2671c141cc:
#include "mbed.h" #include "Motor.h" #include "MCP.h" #include "XBee.h" #include <stdint.h> #include <stdio.h> #include <math.h> #define SDA PB_7 #define SCL PB_6 #define MCP_ADDRESS 0x40 MCP MCP(SDA, SCL, MCP_ADDRESS); XBEE::ControllerData *controller; MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM]; Serial pc(USBTX,USBRX); /* ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ */ #define omni 0 #define mekanamuR 1 #define mekanamuL 2 long map(long x, long in_min, long in_max, long out_min, long out_max) { return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; } void outputMecani(double right_value, double left_value, double omni_value); // メカニ平行移動数 void driveMecani(double power, double degree) { //powerには、 出力の大きさを0~255で入れてください。255で最大です。 //degreeには、進みたい角度を0~360で入れてください。0で右進です。 double right_value;//右メカナムの値 double left_value;//左メカナムの値 double omni_value;//オムニの値 double pa;//前後の速度 double pb;//左右の速度 pa = -(sin((degree)) * fabs(sin((degree))) * power); pb = cos((degree)) * fabs(cos((degree))) * power; right_value = -(pa + pb); left_value = -(pa - pb); omni_value = -pb; //if (left_value < 0) { // left_value = left_value * 0.95; // } else { left_value = left_value * 0.85; right_value = right_value * 0.85; // } outputMecani(right_value, left_value, omni_value); } // メカニ旋回関数 void turnMecani(double power, double center) { //powerには、 出力の大きさを-255~255で入れてください。255で最大です。 //centerには、回転の中心の位置を-255~255で入れてください。中心の位置は、値が大きいとメカナム寄り、小さいとオムニ寄りになります。 double right_value; //右メカナムの値 double left_value; //左メカナムの値 double omni_value; //オムニの値 if (center <= 0) { omni_value = - power * fabs(center); right_value = power; left_value = -power; } else { omni_value = -power; right_value = power * (center); left_value = -power * (center); } outputMecani(right_value, left_value, omni_value); } // メカニピン出力関数Arduino用 void outputMecani(double right_value, double left_value, double omni_value) { if (right_value < 0) { motor[mekanamuR].dir=BACK; } else { motor[mekanamuR].dir=FOR; } if (left_value < 0) { motor[mekanamuL].dir=FOR; } else { motor[mekanamuL].dir=BACK; } if (omni_value > 0) { motor[omni].dir=BACK; } else { motor[omni].dir=FOR; } double R=fabs(right_value); double L=fabs(left_value); double om=fabs(omni_value); R=map(R,0,255,0,100); L=map(L,0,255,0,100); om=map(om,0,255,0,100); motor[mekanamuR].pwm=R; motor[mekanamuL].pwm=L; motor[omni].pwm=om; //横移動時に曲がってしまう場合は調整してください。 } /* ↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑ */ int main() { XBEE::Controller::Initialize(); MOTOR::Motor::Initialize(); __enable_irq(); /* ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ */ /* ↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑ */ while(1) { controller = XBEE::Controller::GetData(); MCP.Update(); /* ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ */ int A=controller->AnalogL.X-7; int B=controller->AnalogL.Y-7; double shita=atan(double(B)/double(A)); double power=sqrt(double(A)*double(A)+double(B)*double(B)); power=map(power,0,7.0,0,255); pc.printf("\r\n"); if(A<0) { if(B>0) { shita=3.141592-atan(fabs(double(B/A))); } else if(B<0) { shita=-3.141592+atan(fabs(double(B/A))); } else if(B==0) { shita=3.141592; } } driveMecani(power,shita); if(A==0&&B==0) { motor[omni].dir=BRAKE; motor[omni].pwm=100; motor[mekanamuR].dir=BRAKE; motor[mekanamuR].pwm=100; motor[mekanamuL].dir=BRAKE; motor[mekanamuL].pwm=100; int turn=controller->AnalogR.X; turn=map(turn,0,14,-255,255); turnMecani(turn,1); } else if(A==0&&B>0) { motor[omni].dir=BRAKE; motor[omni].pwm=100; motor[mekanamuR].dir=FOR; motor[mekanamuR].pwm=power; motor[mekanamuL].dir=BACK; motor[mekanamuL].pwm=power; } else if(A==0&&B<0) { motor[omni].dir=BRAKE; motor[omni].pwm=100; motor[mekanamuR].dir=BACK; motor[mekanamuR].pwm=power; motor[mekanamuL].dir=FOR; motor[mekanamuL].pwm=power; } pc.printf("right=%lf,left=%lf,omni=%lf",motor[mekanamuR].pwm,motor[mekanamuL].pwm,motor[omni].pwm); /* ↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑ */ MOTOR::Motor::Update(motor); } }