toshi mura / Mbed 2 deprecated NRP2020_main

gjyrjyykkudt

Dependencies:   mbed MCP23017

main.cpp@5:8a0663298e2c, 2020-01-25 (annotated)

Committer:
muratoshi
Date:
Sat Jan 25 06:38:16 2020 +0000
Revision:
5:8a0663298e2c
Parent:
3:8790e69f8c59
Child:
6:cd2671c141cc
keugfek

Who changed what in which revision?

UserRevisionLine numberNew contents of line
M_souta 0:db8d4af513c0 1 #include "mbed.h"
M_souta 0:db8d4af513c0 2 #include "Motor.h"
M_souta 0:db8d4af513c0 3 #include "MCP.h"
M_souta 0:db8d4af513c0 4 #include "XBee.h"
M_souta 3:8790e69f8c59 5 #include <stdint.h>
muratoshi 5:8a0663298e2c 6 #include <stdio.h>
muratoshi 5:8a0663298e2c 7 #include <math.h>
M_souta 0:db8d4af513c0 8 #define SDA PB_7
M_souta 0:db8d4af513c0 9 #define SCL PB_6
M_souta 0:db8d4af513c0 10 #define MCP_ADDRESS 0x40
M_souta 0:db8d4af513c0 11
M_souta 0:db8d4af513c0 12 MCP MCP(SDA, SCL, MCP_ADDRESS);
M_souta 0:db8d4af513c0 13 XBEE::ControllerData *controller;
M_souta 2:32d2cd7d744b 14 MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
M_souta 0:db8d4af513c0 15
M_souta 3:8790e69f8c59 16 /* ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ */
M_souta 3:8790e69f8c59 17
muratoshi 5:8a0663298e2c 18 #define omni 0
muratoshi 5:8a0663298e2c 19 #define mekanamuR 1
muratoshi 5:8a0663298e2c 20 #define mekanamuL 2
M_souta 3:8790e69f8c59 21
muratoshi 5:8a0663298e2c 22 long map(long x, long in_min, long in_max, long out_min, long out_max) {
muratoshi 5:8a0663298e2c 23 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
muratoshi 5:8a0663298e2c 24 }
muratoshi 5:8a0663298e2c 25 void outputMecani(double right_value, double left_value, double omni_value);
muratoshi 5:8a0663298e2c 26 // メカニ平行移動数
muratoshi 5:8a0663298e2c 27 void driveMecani(double power, double degree) {
muratoshi 5:8a0663298e2c 28 //powerには、 出力の大きさを0～255で入れてください。255で最大です。
muratoshi 5:8a0663298e2c 29 //degreeには、進みたい角度を0～360で入れてください。0で右進です。
muratoshi 5:8a0663298e2c 30
muratoshi 5:8a0663298e2c 31 double right_value;//右メカナムの値
muratoshi 5:8a0663298e2c 32 double left_value;//左メカナムの値
muratoshi 5:8a0663298e2c 33 double omni_value;//オムニの値
muratoshi 5:8a0663298e2c 34 double pa;//前後の速度
muratoshi 5:8a0663298e2c 35 double pb;//左右の速度
muratoshi 5:8a0663298e2c 36
muratoshi 5:8a0663298e2c 37 pa = -(sin((degree)) * fabs(sin((degree))) * power);
muratoshi 5:8a0663298e2c 38 pb = cos((degree)) * fabs(cos((degree))) * power;
muratoshi 5:8a0663298e2c 39 right_value = -(pa + pb);
muratoshi 5:8a0663298e2c 40 left_value = -(pa - pb);
muratoshi 5:8a0663298e2c 41 omni_value = -pb * 0.75;
muratoshi 5:8a0663298e2c 42 //if (left_value < 0) {
muratoshi 5:8a0663298e2c 43 // left_value = left_value * 0.95;
muratoshi 5:8a0663298e2c 44 // } else {
muratoshi 5:8a0663298e2c 45 right_value = right_value * 0.95;
muratoshi 5:8a0663298e2c 46 // }
muratoshi 5:8a0663298e2c 47 outputMecani(right_value, left_value, omni_value);
muratoshi 5:8a0663298e2c 48
muratoshi 5:8a0663298e2c 49 }
muratoshi 5:8a0663298e2c 50
muratoshi 5:8a0663298e2c 51 // メカニ旋回関数
muratoshi 5:8a0663298e2c 52 void turnMecani(double power, double center) {
muratoshi 5:8a0663298e2c 53 //powerには、 出力の大きさを-255～255で入れてください。255で最大です。
muratoshi 5:8a0663298e2c 54 //centerには、回転の中心の位置を-255~255で入れてください。中心の位置は、値が大きいとメカナム寄り、小さいとオムニ寄りになります。
muratoshi 5:8a0663298e2c 55
muratoshi 5:8a0663298e2c 56 double right_value; //右メカナムの値
muratoshi 5:8a0663298e2c 57 double left_value; //左メカナムの値
muratoshi 5:8a0663298e2c 58 double omni_value; //オムニの値
muratoshi 5:8a0663298e2c 59
muratoshi 5:8a0663298e2c 60 if (center <= 0) {
muratoshi 5:8a0663298e2c 61 omni_value = - power * fabs(center);
muratoshi 5:8a0663298e2c 62 right_value = power;
muratoshi 5:8a0663298e2c 63 left_value = -power;
muratoshi 5:8a0663298e2c 64 } else {
muratoshi 5:8a0663298e2c 65 omni_value = -power;
muratoshi 5:8a0663298e2c 66 right_value = power * (center);
muratoshi 5:8a0663298e2c 67 left_value = -power * (center);
muratoshi 5:8a0663298e2c 68 }
muratoshi 5:8a0663298e2c 69
muratoshi 5:8a0663298e2c 70 outputMecani(right_value, left_value, omni_value);
muratoshi 5:8a0663298e2c 71 }
muratoshi 5:8a0663298e2c 72
muratoshi 5:8a0663298e2c 73 // メカニピン出力関数Arduino用
muratoshi 5:8a0663298e2c 74 void outputMecani(double right_value, double left_value, double omni_value) {
muratoshi 5:8a0663298e2c 75
muratoshi 5:8a0663298e2c 76 if (right_value < 0) {
muratoshi 5:8a0663298e2c 77 motor[mekanamuR].dir=BACK;
muratoshi 5:8a0663298e2c 78 } else {
muratoshi 5:8a0663298e2c 79 motor[mekanamuR].dir=FOR;
muratoshi 5:8a0663298e2c 80 }
muratoshi 5:8a0663298e2c 81
muratoshi 5:8a0663298e2c 82 if (left_value < 0) {
muratoshi 5:8a0663298e2c 83 motor[mekanamuL].dir=FOR;
muratoshi 5:8a0663298e2c 84 } else {
muratoshi 5:8a0663298e2c 85 motor[mekanamuL].dir=BACK;
muratoshi 5:8a0663298e2c 86 }
muratoshi 5:8a0663298e2c 87
muratoshi 5:8a0663298e2c 88 if (omni_value > 0) {
muratoshi 5:8a0663298e2c 89 motor[omni].dir=BACK;
muratoshi 5:8a0663298e2c 90 } else {
muratoshi 5:8a0663298e2c 91 motor[omni].dir=FOR;
muratoshi 5:8a0663298e2c 92 }
muratoshi 5:8a0663298e2c 93 motor[mekanamuR].pwm=fabs(right_value);
muratoshi 5:8a0663298e2c 94 motor[mekanamuL].pwm=fabs(left_value);
muratoshi 5:8a0663298e2c 95 motor[omni].pwm=fabs(omni_value);
muratoshi 5:8a0663298e2c 96 //横移動時に曲がってしまう場合は調整してください。
muratoshi 5:8a0663298e2c 97
muratoshi 5:8a0663298e2c 98 }
muratoshi 5:8a0663298e2c 99
M_souta 3:8790e69f8c59 100 /* ↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑ */
M_souta 3:8790e69f8c59 101
M_souta 0:db8d4af513c0 102 int main() {
M_souta 0:db8d4af513c0 103
M_souta 3:8790e69f8c59 104 XBEE::Controller::Initialize();
M_souta 2:32d2cd7d744b 105 MOTOR::Motor::Initialize();
M_souta 2:32d2cd7d744b 106
M_souta 3:8790e69f8c59 107 __enable_irq();
M_souta 3:8790e69f8c59 108
M_souta 3:8790e69f8c59 109 /* ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ */
M_souta 3:8790e69f8c59 110
M_souta 2:32d2cd7d744b 111
M_souta 1:5b0303768126 112
M_souta 3:8790e69f8c59 113 /* ↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑ */
M_souta 3:8790e69f8c59 114
M_souta 0:db8d4af513c0 115 while(1) {
M_souta 3:8790e69f8c59 116
M_souta 0:db8d4af513c0 117 controller = XBEE::Controller::GetData();
M_souta 1:5b0303768126 118 MCP.Update();
M_souta 0:db8d4af513c0 119
M_souta 3:8790e69f8c59 120 /* ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ */
M_souta 3:8790e69f8c59 121
muratoshi 5:8a0663298e2c 122 int A=controller->AnalogL.X-7;
muratoshi 5:8a0663298e2c 123 int B=controller->AnalogL.Y-7;
muratoshi 5:8a0663298e2c 124 float shita=atan(double(B)/double(A));
muratoshi 5:8a0663298e2c 125 double power=sqrt(double(A)*double(A)+double(B)*double(B));
muratoshi 5:8a0663298e2c 126 power=map(power,0,9.90,0,255);
M_souta 2:32d2cd7d744b 127
muratoshi 5:8a0663298e2c 128 if(A<0&&B<0){
muratoshi 5:8a0663298e2c 129 shita=shita+3.141592f;
muratoshi 5:8a0663298e2c 130 }else if(A<0&&B>0){
muratoshi 5:8a0663298e2c 131 shita=shita+3.141592/2;
muratoshi 5:8a0663298e2c 132 }
muratoshi 5:8a0663298e2c 133 if(A==0){
muratoshi 5:8a0663298e2c 134 if(B>0){
muratoshi 5:8a0663298e2c 135 shita=3.141592;
muratoshi 5:8a0663298e2c 136 }else if(B<0){shita=-3.141592;
muratoshi 5:8a0663298e2c 137 }else if(B==0){shita=0;
muratoshi 5:8a0663298e2c 138 }
muratoshi 5:8a0663298e2c 139 }
muratoshi 5:8a0663298e2c 140
M_souta 2:32d2cd7d744b 141
M_souta 3:8790e69f8c59 142 /* ↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑ */
M_souta 2:32d2cd7d744b 143
M_souta 2:32d2cd7d744b 144 MOTOR::Motor::Update(motor);
M_souta 0:db8d4af513c0 145 }
M_souta 0:db8d4af513c0 146 }