ttt ko / Mbed 2 deprecated NRP2020_main_1_29

aa

Dependencies:   mbed MCP23017

main.cpp@5:4b841637618e, 2020-01-29 (annotated)

Committer:
tttko
Date:
Wed Jan 29 07:03:39 2020 +0000
Revision:
5:4b841637618e
Parent:
3:8790e69f8c59 
aa

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>
M_souta 0:db8d4af513c0 6
tttko 5:4b841637618e 7 #define SDA PB_7
M_souta 0:db8d4af513c0 8 #define SCL PB_6
M_souta 0:db8d4af513c0 9 #define MCP_ADDRESS 0x40
M_souta 0:db8d4af513c0 10
M_souta 0:db8d4af513c0 11 MCP MCP(SDA, SCL, MCP_ADDRESS);
M_souta 0:db8d4af513c0 12 XBEE::ControllerData *controller;
M_souta 2:32d2cd7d744b 13 MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
M_souta 0:db8d4af513c0 14
M_souta 3:8790e69f8c59 15 /* ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ */
tttko 5:4b841637618e 16 //ピン名変更
tttko 5:4b841637618e 17 #define FL 0
tttko 5:4b841637618e 18 #define FR 1
tttko 5:4b841637618e 19 #define BL 2
tttko 5:4b841637618e 20 #define BR 3
tttko 5:4b841637618e 21 #define CC 4
tttko 5:4b841637618e 22 #define Lim0 0
tttko 5:4b841637618e 23 #define Lim1 1
tttko 5:4b841637618e 24 #define AIR0 2
tttko 5:4b841637618e 25 #define AIR1 3
tttko 5:4b841637618e 26 #define AIR2 4
tttko 5:4b841637618e 27 #define SL0 5
tttko 5:4b841637618e 28 #define SL1 6
tttko 5:4b841637618e 29
tttko 5:4b841637618e 30 //リミットスイッチ　読み取る
tttko 5:4b841637618e 31 int g = MCP.Read(0);
tttko 5:4b841637618e 32 const int omni[15][15] = {
tttko 5:4b841637618e 33 { 0, 5, 21, 47, 83, 130, 187, 255, 255, 255, 255, 255, 255, 255, 255 },
tttko 5:4b841637618e 34 { -5, 0, 5, 21, 47, 83, 130, 187, 193, 208, 234, 255, 255, 255, 255 },
tttko 5:4b841637618e 35 { -21, -5, 0, 5, 21, 47, 83, 130, 135, 151, 177, 213, 255, 255, 255 },
tttko 5:4b841637618e 36 { -47, -21, 5, 0, 5, 21, 47, 83, 88, 104, 130, 167, 213, 255, 255 },
tttko 5:4b841637618e 37 { -83, -47, -21, 5, 0, 5, 21, 47, 52, 68, 94, 130, 177, 234, 255 },
tttko 5:4b841637618e 38 { -130, -83, -47, -21, 5, 0, 5, 21, 26, 42, 68, 104, 151, 208, 255 },
tttko 5:4b841637618e 39 { -187, -130, -83, -47, -21, -5, 0, 5, 10, 26, 52, 88, 135, 193, 255 },
tttko 5:4b841637618e 40 { -255, -187, -130, -83, -47, -21, -5, 0, 5, 21, 47, 83, 130, 187, 255 },
tttko 5:4b841637618e 41 { -255, -193, -135, -88, -52, -26, -10, -5, 0, 5, 21, 47, 83, 130, 187 },
tttko 5:4b841637618e 42 { -255, -208, -151, -104, -68, -42, -26, -21, -5, 0, 5, 21, 47, 83, 130 },
tttko 5:4b841637618e 43 { -255, -234, -177, -130, -94, -68, -52, -47, -21, -7, 0, 7, 21, 47, 83 },
tttko 5:4b841637618e 44 { -255, -255, -213, -167, -130, -104, -88, -83, -47, -21, -5, 0, 5, 21, 47 },
tttko 5:4b841637618e 45 { -255, -255, -255, -213, -177, -151, -135, -130, -83, -47, -21, -5, 0, 5, 21 },
tttko 5:4b841637618e 46 { -255, -255, -255, -255, -234, -208, -193, -187, -130, -83, -47, -21, -5, 0, 5 },
tttko 5:4b841637618e 47 { -255, -255, -255, -255, -255, -255, -255, -255, -187, -130, -83, -47, -21, -5, 0 }
tttko 5:4b841637618e 48 };
tttko 5:4b841637618e 49
tttko 5:4b841637618e 50 const int curve[15] = { -204, -150, -104, -66, -38, -17, -4, 0, 4, 17, 38, 66, 104, 150, 204 };
tttko 5:4b841637618e 51
tttko 5:4b841637618e 52 //uint8_t SetStatus(int pwmVal);
tttko 5:4b841637618e 53 //uint8_t SetPWM(int pwmVal);
M_souta 3:8790e69f8c59 54 /* ↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑ */
M_souta 3:8790e69f8c59 55
tttko 5:4b841637618e 56 int main()
tttko 5:4b841637618e 57 {
tttko 5:4b841637618e 58
M_souta 3:8790e69f8c59 59 XBEE::Controller::Initialize();
M_souta 2:32d2cd7d744b 60 MOTOR::Motor::Initialize();
tttko 5:4b841637618e 61
tttko 5:4b841637618e 62 __enable_irq();
tttko 5:4b841637618e 63
M_souta 3:8790e69f8c59 64 /* ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ */
tttko 5:4b841637618e 65
tttko 5:4b841637618e 66
tttko 5:4b841637618e 67
M_souta 3:8790e69f8c59 68 /* ↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑ */
tttko 5:4b841637618e 69
M_souta 0:db8d4af513c0 70 while(1) {
tttko 5:4b841637618e 71
M_souta 0:db8d4af513c0 72 controller = XBEE::Controller::GetData();
M_souta 1:5b0303768126 73 MCP.Update();
tttko 5:4b841637618e 74
M_souta 3:8790e69f8c59 75 /* ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ */
tttko 5:4b841637618e 76 //足回り
tttko 5:4b841637618e 77 //左スティック　向き + 右スティック　向き
tttko 5:4b841637618e 78 motor[FL].dir = MOTOR::Motor::SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X] + curve[controller->AnalogR.X]);
tttko 5:4b841637618e 79 motor[FR].dir = MOTOR::Motor::SetStatus(omni[14-controller->AnalogL.Y][controller->AnalogL.X] +curve[controller->AnalogR.X]);
tttko 5:4b841637618e 80 motor[BL].dir=MOTOR::Motor::SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X] + curve[controller->AnalogR.X]);
tttko 5:4b841637618e 81 motor[BR].dir=MOTOR::Motor::SetStatus(omni[14-controller->AnalogL.Y][controller->AnalogL.X] + curve[14-controller->AnalogR.X]);
tttko 5:4b841637618e 82 //左スティック　PWM + 右スティック　PWM
tttko 5:4b841637618e 83 motor[FL].pwm=MOTOR::Motor::SetPWM(omni[controller->AnalogL.Y][controller->AnalogL.X] + curve[controller->AnalogR.X]);
tttko 5:4b841637618e 84 motor[FR].pwm=MOTOR::Motor::SetPWM(omni[14-controller->AnalogL.Y][controller->AnalogL.X] + curve[14-controller->AnalogR.X]);
tttko 5:4b841637618e 85 motor[BR].pwm=MOTOR::Motor::SetPWM(omni[controller->AnalogL.Y][controller->AnalogL.X] + curve[controller->AnalogR.X]);
tttko 5:4b841637618e 86 motor[BL].pwm=MOTOR::Motor::SetPWM(omni[14-controller->AnalogL.Y][controller->AnalogL.X] + curve[14-controller->AnalogR.X]);
tttko 5:4b841637618e 87 //回収
tttko 5:4b841637618e 88 if(controller-> Button.A) {
tttko 5:4b841637618e 89 motor[CC].pwm=50;
tttko 5:4b841637618e 90 motor[CC].dir = FOR;
tttko 5:4b841637618e 91 }
tttko 5:4b841637618e 92 else if(controller-> Button.B) {
tttko 5:4b841637618e 93 motor[CC].pwm=50;
tttko 5:4b841637618e 94 motor[CC].dir=BACK;
tttko 5:4b841637618e 95 }
tttko 5:4b841637618e 96 else {
tttko 5:4b841637618e 97 motor[CC].pwm=100;
tttko 5:4b841637618e 98 motor[CC].dir=BRAKE;
tttko 5:4b841637618e 99 }
tttko 5:4b841637618e 100 //リミットスイッチのプログラム
tttko 5:4b841637618e 101 if(Lim0==1 || Lim1==1) {
tttko 5:4b841637618e 102 motor[CC].pwm=100;
tttko 5:4b841637618e 103 motor[CC].dir=BRAKE;
tttko 5:4b841637618e 104 }
tttko 5:4b841637618e 105
tttko 5:4b841637618e 106 //投射
tttko 5:4b841637618e 107 if(controller-> Button.Y) {
tttko 5:4b841637618e 108 MCP.Write(AIR2,1);
tttko 5:4b841637618e 109 }
tttko 5:4b841637618e 110 else {
tttko 5:4b841637618e 111 MCP.Write(AIR2,0);
tttko 5:4b841637618e 112 }
tttko 5:4b841637618e 113 //スロープ
tttko 5:4b841637618e 114 if(controller-> Button.L) {
tttko 5:4b841637618e 115 MCP.Write(AIR1,1);
tttko 5:4b841637618e 116 }
tttko 5:4b841637618e 117 else {
tttko 5:4b841637618e 118 MCP.Write(AIR1,0);
tttko 5:4b841637618e 119 }
tttko 5:4b841637618e 120
tttko 5:4b841637618e 121 if(controller-> Button.ZL) {
tttko 5:4b841637618e 122 MCP.Write(SL1,1);
tttko 5:4b841637618e 123 }
tttko 5:4b841637618e 124 else {
tttko 5:4b841637618e 125 MCP.Write(SL1,0);
tttko 5:4b841637618e 126 }
tttko 5:4b841637618e 127
tttko 5:4b841637618e 128 //ストックボックス
tttko 5:4b841637618e 129 if(controller-> Button.R) {
tttko 5:4b841637618e 130 MCP.Write(AIR0,1);
tttko 5:4b841637618e 131 }
tttko 5:4b841637618e 132 else {
tttko 5:4b841637618e 133 MCP.Write(AIR0,0);
tttko 5:4b841637618e 134 }
tttko 5:4b841637618e 135
tttko 5:4b841637618e 136 if(controller-> Button.ZR) {
tttko 5:4b841637618e 137 MCP.Write(SL0,1);
tttko 5:4b841637618e 138 }
tttko 5:4b841637618e 139 else {
tttko 5:4b841637618e 140 MCP.Write(SL0,0);
tttko 5:4b841637618e 141 }
tttko 5:4b841637618e 142
tttko 5:4b841637618e 143 /* ↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑ */
tttko 5:4b841637618e 144
tttko 5:4b841637618e 145 MOTOR::Motor::Update(motor);
tttko 5:4b841637618e 146 }
M_souta 0:db8d4af513c0 147 }