toshi mura / Mbed 2 deprecated NRP2020_main

gjyrjyykkudt

Dependencies:   mbed MCP23017

main.cpp@6:cd2671c141cc, 2020-02-01 (annotated)

Committer:
muratoshi
Date:
Sat Feb 01 06:40:55 2020 +0000
Revision:
6:cd2671c141cc
Parent:
5:8a0663298e2c 
kaljrgaogivrjnae:

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>
muratoshi 6:cd2671c141cc 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];
muratoshi 6:cd2671c141cc 15 Serial pc(USBTX,USBRX);
M_souta 3:8790e69f8c59 16 /* ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ */
muratoshi 6:cd2671c141cc 17
muratoshi 5:8a0663298e2c 18 #define omni 0
muratoshi 5:8a0663298e2c 19 #define mekanamuR 1
muratoshi 5:8a0663298e2c 20 #define mekanamuL 2
muratoshi 6:cd2671c141cc 21
muratoshi 6:cd2671c141cc 22 long map(long x, long in_min, long in_max, long out_min, long out_max)
muratoshi 6:cd2671c141cc 23 {
muratoshi 6:cd2671c141cc 24 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
muratoshi 5:8a0663298e2c 25 }
muratoshi 5:8a0663298e2c 26 void outputMecani(double right_value, double left_value, double omni_value);
muratoshi 5:8a0663298e2c 27 // メカニ平行移動数
muratoshi 6:cd2671c141cc 28 void driveMecani(double power, double degree)
muratoshi 6:cd2671c141cc 29 {
muratoshi 6:cd2671c141cc 30 //powerには、 出力の大きさを0～255で入れてください。255で最大です。
muratoshi 6:cd2671c141cc 31 //degreeには、進みたい角度を0～360で入れてください。0で右進です。
muratoshi 5:8a0663298e2c 32
muratoshi 6:cd2671c141cc 33 double right_value;//右メカナムの値
muratoshi 6:cd2671c141cc 34 double left_value;//左メカナムの値
muratoshi 6:cd2671c141cc 35 double omni_value;//オムニの値
muratoshi 6:cd2671c141cc 36 double pa;//前後の速度
muratoshi 6:cd2671c141cc 37 double pb;//左右の速度
muratoshi 5:8a0663298e2c 38
muratoshi 6:cd2671c141cc 39 pa = -(sin((degree)) * fabs(sin((degree))) * power);
muratoshi 6:cd2671c141cc 40 pb = cos((degree)) * fabs(cos((degree))) * power;
muratoshi 6:cd2671c141cc 41 right_value = -(pa + pb);
muratoshi 6:cd2671c141cc 42 left_value = -(pa - pb);
muratoshi 6:cd2671c141cc 43 omni_value = -pb;
muratoshi 6:cd2671c141cc 44 //if (left_value < 0) {
muratoshi 6:cd2671c141cc 45 // left_value = left_value * 0.95;
muratoshi 6:cd2671c141cc 46 // } else {
muratoshi 6:cd2671c141cc 47 left_value = left_value * 0.85;
muratoshi 6:cd2671c141cc 48 right_value = right_value * 0.85;
muratoshi 6:cd2671c141cc 49 // }
muratoshi 6:cd2671c141cc 50 outputMecani(right_value, left_value, omni_value);
muratoshi 5:8a0663298e2c 51
muratoshi 5:8a0663298e2c 52 }
muratoshi 5:8a0663298e2c 53
muratoshi 5:8a0663298e2c 54 // メカニ旋回関数
muratoshi 6:cd2671c141cc 55 void turnMecani(double power, double center)
muratoshi 6:cd2671c141cc 56 {
muratoshi 6:cd2671c141cc 57 //powerには、 出力の大きさを-255～255で入れてください。255で最大です。
muratoshi 6:cd2671c141cc 58 //centerには、回転の中心の位置を-255~255で入れてください。中心の位置は、値が大きいとメカナム寄り、小さいとオムニ寄りになります。
muratoshi 5:8a0663298e2c 59
muratoshi 6:cd2671c141cc 60 double right_value; //右メカナムの値
muratoshi 6:cd2671c141cc 61 double left_value; //左メカナムの値
muratoshi 6:cd2671c141cc 62 double omni_value; //オムニの値
muratoshi 5:8a0663298e2c 63
muratoshi 6:cd2671c141cc 64 if (center <= 0) {
muratoshi 6:cd2671c141cc 65 omni_value = - power * fabs(center);
muratoshi 6:cd2671c141cc 66 right_value = power;
muratoshi 6:cd2671c141cc 67 left_value = -power;
muratoshi 6:cd2671c141cc 68 } else {
muratoshi 6:cd2671c141cc 69 omni_value = -power;
muratoshi 6:cd2671c141cc 70 right_value = power * (center);
muratoshi 6:cd2671c141cc 71 left_value = -power * (center);
muratoshi 6:cd2671c141cc 72 }
muratoshi 5:8a0663298e2c 73
muratoshi 6:cd2671c141cc 74 outputMecani(right_value, left_value, omni_value);
muratoshi 5:8a0663298e2c 75 }
muratoshi 5:8a0663298e2c 76
muratoshi 5:8a0663298e2c 77 // メカニピン出力関数Arduino用
muratoshi 6:cd2671c141cc 78 void outputMecani(double right_value, double left_value, double omni_value)
muratoshi 6:cd2671c141cc 79 {
muratoshi 5:8a0663298e2c 80
muratoshi 6:cd2671c141cc 81 if (right_value < 0) {
muratoshi 6:cd2671c141cc 82 motor[mekanamuR].dir=BACK;
muratoshi 6:cd2671c141cc 83 } else {
muratoshi 6:cd2671c141cc 84 motor[mekanamuR].dir=FOR;
muratoshi 6:cd2671c141cc 85 }
muratoshi 6:cd2671c141cc 86
muratoshi 6:cd2671c141cc 87 if (left_value < 0) {
muratoshi 6:cd2671c141cc 88 motor[mekanamuL].dir=FOR;
muratoshi 6:cd2671c141cc 89 } else {
muratoshi 6:cd2671c141cc 90 motor[mekanamuL].dir=BACK;
muratoshi 6:cd2671c141cc 91 }
muratoshi 5:8a0663298e2c 92
muratoshi 6:cd2671c141cc 93 if (omni_value > 0) {
muratoshi 6:cd2671c141cc 94 motor[omni].dir=BACK;
muratoshi 6:cd2671c141cc 95 } else {
muratoshi 6:cd2671c141cc 96 motor[omni].dir=FOR;
muratoshi 6:cd2671c141cc 97 }
muratoshi 6:cd2671c141cc 98
muratoshi 6:cd2671c141cc 99 double R=fabs(right_value);
muratoshi 6:cd2671c141cc 100 double L=fabs(left_value);
muratoshi 6:cd2671c141cc 101 double om=fabs(omni_value);
muratoshi 6:cd2671c141cc 102 R=map(R,0,255,0,100);
muratoshi 6:cd2671c141cc 103 L=map(L,0,255,0,100);
muratoshi 6:cd2671c141cc 104 om=map(om,0,255,0,100);
muratoshi 6:cd2671c141cc 105 motor[mekanamuR].pwm=R;
muratoshi 6:cd2671c141cc 106 motor[mekanamuL].pwm=L;
muratoshi 6:cd2671c141cc 107 motor[omni].pwm=om;
muratoshi 5:8a0663298e2c 108
muratoshi 6:cd2671c141cc 109 //横移動時に曲がってしまう場合は調整してください。
muratoshi 5:8a0663298e2c 110
muratoshi 5:8a0663298e2c 111 }
muratoshi 5:8a0663298e2c 112
M_souta 3:8790e69f8c59 113 /* ↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑ */
M_souta 3:8790e69f8c59 114
muratoshi 6:cd2671c141cc 115 int main()
muratoshi 6:cd2671c141cc 116 {
muratoshi 6:cd2671c141cc 117
M_souta 3:8790e69f8c59 118 XBEE::Controller::Initialize();
M_souta 2:32d2cd7d744b 119 MOTOR::Motor::Initialize();
muratoshi 6:cd2671c141cc 120
muratoshi 6:cd2671c141cc 121 __enable_irq();
muratoshi 6:cd2671c141cc 122
M_souta 3:8790e69f8c59 123 /* ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ */
muratoshi 6:cd2671c141cc 124
muratoshi 6:cd2671c141cc 125
muratoshi 6:cd2671c141cc 126
M_souta 3:8790e69f8c59 127 /* ↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑ */
muratoshi 6:cd2671c141cc 128
M_souta 0:db8d4af513c0 129 while(1) {
muratoshi 6:cd2671c141cc 130
M_souta 0:db8d4af513c0 131 controller = XBEE::Controller::GetData();
M_souta 1:5b0303768126 132 MCP.Update();
muratoshi 6:cd2671c141cc 133
M_souta 3:8790e69f8c59 134 /* ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ */
muratoshi 6:cd2671c141cc 135
muratoshi 5:8a0663298e2c 136 int A=controller->AnalogL.X-7;
muratoshi 5:8a0663298e2c 137 int B=controller->AnalogL.Y-7;
muratoshi 6:cd2671c141cc 138 double shita=atan(double(B)/double(A));
muratoshi 5:8a0663298e2c 139 double power=sqrt(double(A)*double(A)+double(B)*double(B));
muratoshi 6:cd2671c141cc 140 power=map(power,0,7.0,0,255);
muratoshi 6:cd2671c141cc 141 pc.printf("\r\n");
muratoshi 6:cd2671c141cc 142
muratoshi 6:cd2671c141cc 143 if(A<0) {
muratoshi 6:cd2671c141cc 144 if(B>0) {
muratoshi 6:cd2671c141cc 145 shita=3.141592-atan(fabs(double(B/A)));
muratoshi 6:cd2671c141cc 146 } else if(B<0) {
muratoshi 6:cd2671c141cc 147 shita=-3.141592+atan(fabs(double(B/A)));
muratoshi 6:cd2671c141cc 148 } else if(B==0) {
muratoshi 6:cd2671c141cc 149 shita=3.141592;
muratoshi 5:8a0663298e2c 150 }
muratoshi 6:cd2671c141cc 151 }
muratoshi 6:cd2671c141cc 152
muratoshi 6:cd2671c141cc 153
muratoshi 6:cd2671c141cc 154 driveMecani(power,shita);
M_souta 2:32d2cd7d744b 155
muratoshi 6:cd2671c141cc 156 if(A==0&&B==0) {
muratoshi 6:cd2671c141cc 157 motor[omni].dir=BRAKE;
muratoshi 6:cd2671c141cc 158 motor[omni].pwm=100;
muratoshi 6:cd2671c141cc 159 motor[mekanamuR].dir=BRAKE;
muratoshi 6:cd2671c141cc 160 motor[mekanamuR].pwm=100;
muratoshi 6:cd2671c141cc 161 motor[mekanamuL].dir=BRAKE;
muratoshi 6:cd2671c141cc 162 motor[mekanamuL].pwm=100;
muratoshi 6:cd2671c141cc 163 int turn=controller->AnalogR.X;
muratoshi 6:cd2671c141cc 164 turn=map(turn,0,14,-255,255);
muratoshi 6:cd2671c141cc 165 turnMecani(turn,1);
muratoshi 6:cd2671c141cc 166 } else if(A==0&&B>0) {
muratoshi 6:cd2671c141cc 167 motor[omni].dir=BRAKE;
muratoshi 6:cd2671c141cc 168 motor[omni].pwm=100;
muratoshi 6:cd2671c141cc 169 motor[mekanamuR].dir=FOR;
muratoshi 6:cd2671c141cc 170 motor[mekanamuR].pwm=power;
muratoshi 6:cd2671c141cc 171 motor[mekanamuL].dir=BACK;
muratoshi 6:cd2671c141cc 172 motor[mekanamuL].pwm=power;
muratoshi 6:cd2671c141cc 173 } else if(A==0&&B<0) {
muratoshi 6:cd2671c141cc 174 motor[omni].dir=BRAKE;
muratoshi 6:cd2671c141cc 175 motor[omni].pwm=100;
muratoshi 6:cd2671c141cc 176 motor[mekanamuR].dir=BACK;
muratoshi 6:cd2671c141cc 177 motor[mekanamuR].pwm=power;
muratoshi 6:cd2671c141cc 178 motor[mekanamuL].dir=FOR;
muratoshi 6:cd2671c141cc 179 motor[mekanamuL].pwm=power;
muratoshi 6:cd2671c141cc 180 }
muratoshi 6:cd2671c141cc 181 pc.printf("right=%lf,left=%lf,omni=%lf",motor[mekanamuR].pwm,motor[mekanamuL].pwm,motor[omni].pwm);
muratoshi 6:cd2671c141cc 182
muratoshi 6:cd2671c141cc 183
muratoshi 6:cd2671c141cc 184
M_souta 3:8790e69f8c59 185 /* ↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑ */
muratoshi 6:cd2671c141cc 186
M_souta 2:32d2cd7d744b 187 MOTOR::Motor::Update(motor);
M_souta 0:db8d4af513c0 188 }
M_souta 0:db8d4af513c0 189 }