Miya Miyagawa
If you wont to knock me down, look this file...
Revision 0:4df75b08b14a, committed 2019-05-03
- Comitter:
- Ryosei
- Date:
- Fri May 03 11:47:27 2019 +0000
- Commit message:
- a
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Communication/XBee/XBee.cpp Fri May 03 11:47:27 2019 +0000
@@ -0,0 +1,112 @@
+#include "XBee.h"
+
+#include <stdint.h>
+#include "mbed.h"
+
+namespace XBEE
+{
+ Ticker xbee_timer;
+ Serial xbee_uart(XBEE_TX, XBEE_RX);
+ DigitalOut XBee_LED(LED1);
+
+ void uartUpdate(void);
+ void lostCheck(void);
+
+ namespace
+ {
+ ControllerData ctrData;
+ ControllerData keepCtrData;
+ const uint8_t defaultData[4] = CTR_DEFAULT_DATA;
+ const char check[] = "DT=";
+ volatile char packet[24];
+
+ bool controllerLost = false;
+ uint8_t timerCount = 0;
+ }
+
+ void Controller::Initialize(void) {
+ xbee_timer.attach(lostCheck, 0.025);
+ xbee_uart.baud(4800);
+ xbee_uart.attach(uartUpdate, Serial::RxIrq);
+ DataReset();
+ }
+
+ ControllerData* Controller::GetData(void) {
+ __disable_irq();
+ for(uint8_t i = 0; i < CTR_DATA_LENGTH; i++) keepCtrData.buf[i] = ctrData.buf[i];
+ __enable_irq();
+ return &keepCtrData;
+ }
+
+ void Controller::DataReset(void) {
+ __disable_irq();
+ for(uint8_t i = 0; i < CTR_DATA_LENGTH; i++) ctrData.buf[i] = defaultData[i];
+ __enable_irq();
+ }
+
+ bool Controller::CheckControllerLost(void) {
+ return controllerLost;
+ }
+
+ void uartUpdate(void) {
+ static bool phase = false;
+ static uint8_t count = 0;
+
+ char data = xbee_uart.getc();
+
+ if(phase)
+ {
+ packet[count] = data;
+ if(count < 2)
+ {
+ if(data != check[count])
+ {
+ phase = false;
+ controllerLost = true;
+ XBee_LED = LED_OFF;
+ }
+ }
+ else if(count == 8)
+ {
+ if(data != '\r')
+ {
+ phase = false;
+ controllerLost = true;
+ XBee_LED = LED_OFF;
+ }
+ else
+ {
+ ctrData.buf[0] = packet[4];
+ ctrData.buf[1] = packet[5];
+ ctrData.buf[2] = packet[6];
+ ctrData.buf[3] = packet[7];
+ phase = false;
+ timerCount = 0;
+ controllerLost = false;
+ XBee_LED = LED_ON;
+ }
+ }
+ count++;
+ }
+ else
+ {
+ if(data == '@')
+ {
+ count = 0;
+ phase = true;
+ }
+ }
+ }
+
+ void lostCheck(void) {
+ timerCount++;
+ if(timerCount == 2) XBee_LED = LED_OFF;
+ if(timerCount >= 20) {
+ controllerLost = true;
+ Controller::DataReset();
+ timerCount = 0;
+ XBee_LED = LED_OFF;
+ }
+ }
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Communication/XBee/XBee.h Fri May 03 11:47:27 2019 +0000
@@ -0,0 +1,61 @@
+#ifndef XBEE_H_
+#define XBEE_H_
+
+#include <stdint.h>
+
+namespace XBEE
+{
+ #define CTR_DATA_LENGTH 4
+ #define CTR_DEFAULT_DATA {0x00, 0x00, 0x77, 0x77}
+
+ #define XBEE_TX D1
+ #define XBEE_RX D0
+
+ #define LED_OFF 0
+ #define LED_ON 1
+
+ typedef union
+ {
+ struct {
+ struct {
+ unsigned int X:1;
+ unsigned int A:1;
+ unsigned int B:1;
+ unsigned int Y:1;
+ unsigned int UP:1;
+ unsigned int RIGHT:1;
+ unsigned int DOWN:1;
+ unsigned int LEFT:1;
+ unsigned int SELECT:1;
+ unsigned int HOME:1;
+ unsigned int START:1;
+ unsigned int ZL:1;
+ unsigned int ZR:1;
+ unsigned int L:1;
+ unsigned int R:1;
+ unsigned int :1;
+ } __attribute__ ((packed)) Button;
+ struct {
+ unsigned int Y:4;
+ unsigned int X:4;
+ } __attribute__ ((packed)) AnalogL;
+ struct {
+ unsigned int Y:4;
+ unsigned int X:4;
+ } __attribute__ ((packed)) AnalogR;
+ } __attribute__ ((packed)) ;
+ uint8_t buf[CTR_DATA_LENGTH];
+ }ControllerData;
+
+ class Controller
+ {
+ public:
+ static void Initialize(void);
+ static ControllerData* GetData(void);
+ static void DataReset(void);
+ static bool CheckControllerLost(void);
+ };
+}
+
+#endif
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Input/Switch/Switch.cpp Fri May 03 11:47:27 2019 +0000
@@ -0,0 +1,20 @@
+#include "Switch.h"
+#include <stdint.h>
+
+DigitalIn limitSw[] = {
+ DigitalIn(LIMITSW0_PIN),
+ DigitalIn(LIMITSW1_PIN),
+ DigitalIn(LIMITSW2_PIN),
+ DigitalIn(LIMITSW3_PIN),
+};
+
+namespace SWITCH
+{
+ void Switch::Initialize(void) {
+ for(uint8_t i = 0; i < MOUNTING_LIMITSW_NUM; i++) limitSw[i].mode(PullUp);
+ }
+
+ bool Switch::checkPushed(int mySwitch) {
+ return mySwitch ? false : true;
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Input/Switch/Switch.h Fri May 03 11:47:27 2019 +0000
@@ -0,0 +1,25 @@
+#ifndef SWITCH_H_
+#define SWITCH_H_
+
+#include "mbed.h"
+
+extern DigitalIn limitSw[];
+
+namespace SWITCH
+{
+ #define LIMITSW0_PIN A7
+ #define LIMITSW1_PIN A6
+ #define LIMITSW2_PIN A5
+ #define LIMITSW3_PIN A4
+
+ #define MOUNTING_LIMITSW_NUM 4
+
+ class Switch
+ {
+ public:
+ static void Initialize(void);
+ static bool checkPushed(int mySwitch);
+ };
+}
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Output/Motor/Motor.cpp Fri May 03 11:47:27 2019 +0000
@@ -0,0 +1,78 @@
+#include "Motor.h"
+
+#include <stdint.h>
+#include "mbed.h"
+
+namespace MOTOR
+{
+ namespace
+ {
+ MotorStatus motor[MOUNTING_MOTOR_NUM];
+ DigitalOut directions[] = {
+ DigitalOut(MOTOR0_D1_PIN),
+ DigitalOut(MOTOR0_D2_PIN),
+ DigitalOut(MOTOR1_D1_PIN),
+ DigitalOut(MOTOR1_D2_PIN),
+ DigitalOut(MOTOR2_D1_PIN),
+ DigitalOut(MOTOR2_D2_PIN),
+ DigitalOut(MOTOR3_D1_PIN),
+ DigitalOut(MOTOR3_D2_PIN),
+ DigitalOut(MOTOR4_D1_PIN),
+ DigitalOut(MOTOR4_D2_PIN),
+ };
+ PwmOut pwms[] = {
+ PwmOut(MOTOR0_PWM_PIN),
+ PwmOut(MOTOR1_PWM_PIN),
+ PwmOut(MOTOR2_PWM_PIN),
+ PwmOut(MOTOR3_PWM_PIN),
+ PwmOut(MOTOR4_PWM_PIN),
+ };
+ }
+
+ float percentage_to_ratio(float percentage);
+
+ void Motor::Initialize(void) {
+ //Port Initialize
+ for(uint8_t i = MOTOR_START_NUM; i < MOUNTING_MOTOR_NUM * 2; i++) {
+ directions[i] = 0;
+ }
+
+ //Pwm Initialize
+ for(uint8_t i = MOTOR_START_NUM; i < MOUNTING_MOTOR_NUM; i++) {
+ pwms[i].period_us(50); //20kHz
+ pwms[i] = 0.0;
+ }
+
+ SetDefault();
+ }
+
+ void Motor::SetDefault(void) {
+ for(uint8_t i = MOTOR_START_NUM; i < MOUNTING_MOTOR_NUM; i++) {
+ motor[i].dir = FREE;
+ motor[i].pwm = 0;
+ }
+ }
+
+ void Motor::Update(MotorStatus *status) {
+ for(uint8_t i = MOTOR_START_NUM; i < MOUNTING_MOTOR_NUM; i++) motor[i] = status[i];
+
+ //PWM Update
+ for(uint8_t i = MOTOR_START_NUM; i < MOUNTING_MOTOR_NUM; i++) pwms[i] = percentage_to_ratio(motor[i].pwm);
+
+ //Port Update
+ for(uint8_t i = MOTOR_START_NUM; i < MOUNTING_MOTOR_NUM; i++) {
+ directions[i * 2] = motor[i].d1;
+ directions[i * 2 + 1] = motor[i].d2;
+ }
+ }
+
+ float percentage_to_ratio(float percentage) {
+ return percentage / 100.0;
+ }
+
+ int Motor::SetStatus(float pwm) {
+ if(pwm > 0.0) return FOR;
+ else if(pwm < 0.0) return BACK;
+ else return BRAKE;
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Output/Motor/Motor.h Fri May 03 11:47:27 2019 +0000
@@ -0,0 +1,75 @@
+#ifndef MOTOR_H_
+#define MOTOR_H_
+
+#include <stdint.h>
+
+//#define USE_MOTOR0_SENSOR
+namespace MOTOR
+{
+ #define FREE 0
+ #define BACK 1
+ #define FOR 2
+ #define BRAKE 3
+
+ #define MOUNTING_MOTOR_NUM 5
+
+ #define MOTOR0_D1 directions[0]
+ #define MOTOR0_D2 directions[1]
+ #define MOTOR1_D1 directions[2]
+ #define MOTOR1_D2 directions[3]
+ #define MOTOR2_D1 directions[4]
+ #define MOTOR2_D2 directions[5]
+ #define MOTOR3_D1 directions[6]
+ #define MOTOR3_D2 directions[7]
+ #define MOTOR4_D1 directions[8]
+ #define MOTOR4_D2 directions[9]
+
+ #define MOTOR0_D1_PIN D3
+ #define MOTOR0_D2_PIN D4
+ #define MOTOR1_D1_PIN D5
+ #define MOTOR1_D2_PIN D6
+ #define MOTOR2_D1_PIN D8
+ #define MOTOR2_D2_PIN D9
+ #define MOTOR3_D1_PIN D12
+ #define MOTOR3_D2_PIN A3
+ #define MOTOR4_D1_PIN A1
+ #define MOTOR4_D2_PIN A0
+
+ #define MOTOR0_PWM_PIN D2
+ #define MOTOR1_PWM_PIN D7
+ #define MOTOR2_PWM_PIN D10
+ #define MOTOR3_PWM_PIN D11
+ #define MOTOR4_PWM_PIN A2
+
+ #ifdef USE_MOTOR0_SENSOR
+ #define MOTOR_START_NUM 1
+ #else
+ #define MOTOR_START_NUM 0
+ #endif
+
+ typedef struct
+ {
+ union
+ {
+ struct
+ {
+ unsigned int d2 : 1;
+ unsigned int d1 : 1;
+ unsigned int : 6;
+ };
+ uint8_t dir;
+ };
+ float pwm;
+ }MotorStatus;
+
+ class Motor
+ {
+ public:
+ static void Initialize(void);
+ static void Update(MotorStatus *status);
+ static void SetDefault(void);
+ static int SetStatus(float pwm);
+ };
+}
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Output/Servo.lib Fri May 03 11:47:27 2019 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/simon/code/Servo/#36b69a7ced07
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/System/Initialize/Initialize.cpp Fri May 03 11:47:27 2019 +0000
@@ -0,0 +1,16 @@
+#include "Initialize.h"
+
+#include "mbed.h"
+#include "../../Communication/XBee/XBee.h"
+#include "../../Input/Switch/Switch.h"
+#include ".././Output/Motor/Motor.h"
+
+void SystemInitialize(void) {
+ XBEE::Controller::Initialize();
+// SWITCH::Switch::Initialize();
+ MOTOR::Motor::Initialize();
+
+ //割り込み許可
+ __enable_irq();
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/System/Initialize/Initialize.h Fri May 03 11:47:27 2019 +0000 @@ -0,0 +1,7 @@ +#ifndef INITIALIZE_H_ +#define INITIALIZE_H_ + +void SystemInitialize(void); + +#endif +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/System/Process/Process.cpp Fri May 03 11:47:27 2019 +0000
@@ -0,0 +1,538 @@
+
+
+
+/*
+このプログラムを見る人へ
+
+用語解説
+
+ご挨拶モード：足回りでロボットを左右に振りご挨拶しつつ、回路リセットを行うおれたおになくてはならないモード
+足回り：宮川は十字キー足回りを、新村はアナログスティック足回りを担当しました。宮川のプログラムは猫でも書けますが、新村のは難易度S＋です。わからないことがあれば新村へ
+
+わからないことがあればアナログスティック以外、なんでも宮川に聞いてください
+*/
+
+
+
+
+//------------------------------------------------------------------------------------------------------------------------------------------
+#include "Process.h"
+
+#include "mbed.h"
+#include "../../Communication/XBee/XBee.h"
+//#include "../../Input/Switch/Switch.h"
+#include "../../Output/Motor/Motor.h"
+#include "../../Output/Servo/Servo.h"
+
+//_____________________
+/*---------------- HOW TO WRITE ----------------/
+
+ ・motor の割り当てを決める
+ #define TIRE_L 1
+
+ ・リミットスイッチの割り当てを決める
+ #define ARM_L 1
+
+ ・他にも自由に定義してもいいです (pwmとか)
+
+/---------------- HOW TO WRITE ----------------*/
+//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓
+
+
+#define TIRE_1 0
+#define TIRE_2 1
+#define TIRE_3 2
+#define ARM 3
+
+#define ARMlim 0
+
+int Air0=0;
+int Air1=1;
+
+#define a 0
+#define b 1
+#define c 2
+
+int a_array[15][15] = {
+ { -30, -20, -10, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10, 20, 30 },
+ { -40, -30, -20, -10, 0, 0, 0, 0, 0, 0, 0, 10, 20, 30, 40 },
+ { -50, -40, -30, -20, -10, 0, 0, 0, 0, 0, 10, 20, 30, 40, 50 },
+ { -60, -50, -40, -30, -20, -10, 0, 0, 0, 10, 20, 30, 40, 50, 60 },
+ { -70, -60, -50, -40, -30, -20, -10, 0, 10, 20, 30, 40, 50, 60, 70 },
+ { -80, -70, -60, -50, -40, -30, -20, 0, 20, 30, 40, 50, 60, 70, 80 },
+ { -90, -80, -70, -60, -50, -40, -30, 0, 30, 40, 50, 60, 70, 80, 90 },
+ { -100, -90, -80, -70, -60, -50, -40, 0, 40, 50, 60, 70, 80, 90, 100 },
+ { -90, -80, -70, -60, -50, -40, -30, 0, 30, 40, 50, 60, 70, 80, 90 },
+ { -80, -70, -60, -50, -40, -30, -20, 0, 20, 30, 40, 50, 60, 70, 80 },
+ { -70, -60, -50, -40, -30, -20, -10, 0, 10, 20, 30, 40, 50, 60, 70 },
+ { -60, -50, -40, -30, -20, -10, 0, 0, 0, 10, 20, 30, 40, 50, 60 },
+ { -50, -40, -30, -20, -10, 0, 0, 0, 0, 0, 10, 20, 30, 40, 50 },
+ { -40, -30, -20, -10, 0, 0, 0, 0, 0, 0, 0, 10, 20, 30, 40 },
+ { -30, -20, -10, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10, 20, 30 }
+};
+
+int b_array[15][15] = {
+ { 100, 92, 84, 76, 68, 60, 52, 100, 36, 28, 20, 12, 4, 0, 0 },
+ { 92, 84, 76, 68, 60, 52, 44, 36, 28, 20, 12, 4, 0, 0, 0 },
+ { 84, 76, 68, 60, 52, 44, 36, 28, 20, 12, 4, 0, 0, 0, -4 },
+ { 76, 68, 60, 52, 44, 36, 28, 20, 12, 4, 0, 0, 0, -4, -12 },
+ { 68, 60, 52, 44, 36, 28, 20, 12, 4, 0, 0, 0, -4, -12, -20 },
+ { 60, 52, 44, 36, 28, 20, 12, 4, 0, 0, 0, -4, -12, -20, -28 },
+ { 52, 44, 36, 28, 20, 12, 4, 0, 0, 0, -4, -12, -20, -28, -36 },
+ { 100, 36, 28, 20, 12, 4, 0, 0, 0, -4, -12, -20, -28, -36, -44 },
+ { 85, 28, 20, 12, 4, 0, 0, 0, -4, -12, -20, -28, -36, -44, -52 },
+ { 70, 20, 12, 4, 0, 0, 0, -4, -12, -20, -28, -36, -44, -52, -60 },
+ { 55, 12, 4, 0, 0, 0, -4, -12, -20, -28, -36, -44, -52, -60, -68 },
+ { 40, 4, 0, 0, 0, -4, -12, -20, -28, -36, -44, -52, -60, -68, -76 },
+ { 25, 0, 0, 0, -4, -12, -20, -28, -36, -44, -52, -60, -68, -76, -84 },
+ { 10, 0, 0, -4, -12, -20, -28, -36, -44, -52, -60, -68, -76, -84, -92 },
+ { 0, 0, -4, -12, -20, -28, -36, -100, -52, -60, -68, -76, -84, -92, -100 }
+};
+
+
+int c_array[15][15] = {
+ { 0, 0, 4, 12, 20, 28, 36, 100, 52, 60, 68, 76, 84, 92, 100 },
+ { 0, 0, 0, 4, 12, 20, 28, 36, 44, 52, 60, 68, 76, 84, 92 },
+ { -4, 0, 0, 0, 4, 12, 20, 28, 36, 44, 52, 60, 68, 76, 84 },
+ { -12, -4, 0, 0, 0, 4, 12, 20, 28, 36, 44, 52, 60, 68, 76 },
+ { -20, -12, -4, 0, 0, 0, 4, 12, 20, 28, 36, 44, 52, 60, 68 },
+ { -28, -20, -12, -4, 0, 0, 0, 4, 12, 20, 28, 36, 44, 52, 60 },
+ { -36, -28, -20, -12, -4, 0, 0, 0, 4, 12, 20, 28, 36, 44, 52 },
+ { -44, -36, -28, -20, -12, -4, 0, 0, 0, 4, 12, 20, 28, 36, 44 },
+ { -52, -44, -36, -28, -20, -12, 4, 0, 0, 0, 4, 12, 20, 28, 36 },
+ { -60, -52, -44, -36, -28, -20, -12, -4, 0, 0, 0, 4, 12, 20, 28 },
+ { -68, -60, -52, -44, -36, -28, -20, -12, -4, 0, 0, 0, 4, 12, 20 },
+ { -76, -68, -60, -52, -44, -36, -28, -20, -12, -4, 0, 0, 0, 4, 12 },
+ { -84, -76, -68, -60, -52, -44, -36, -28, -20, -12, -4, 0, 0, 0, 4 },
+ { -92, -84, -76, -68, -60, -52, -44, -36, -28, -20, -12, -4, 0, 0, 0 },
+ { -100, -92, -84, -76, -76, -60, -100, -100, -36, -28, -20, -12, -4, 0, 0 }
+};
+
+int pwm_array[15] = { 30, 25, 20, 15, 10, 5, 0, 0, 0, -5, -10, -15, -20, -25, -30 };
+
+#define usiro 0
+#define mae 0
+uint8_t motorData[5];
+uint8_t pwmData[5];
+
+int conlx;
+int conly;
+int conrx;
+int conry;
+int conba;
+int conbb;
+int conbx;
+int conby;
+int homes=0;
+//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑
+//_____________________
+
+//#define USE_USB_SERIAL
+#ifdef USE_USB_SERIAL
+Serial pc(SERIAL_TX, SERIAL_RX);
+#endif
+Serial pc(SERIAL_TX,SERIAL_RX);
+XBEE::ControllerData *controller;
+MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
+//////////////////////////////////
+//関数、タイマーの宣言
+DigitalOut Air[]= {
+ DigitalOut(A5),
+ DigitalOut(A4),
+};
+DigitalIn limit[]= {
+ DigitalIn(A7),
+ DigitalIn(A6),
+};
+Timer Airtime;
+Timer home;
+void AirOut(int pin,int mode)
+{
+ Air[pin]=mode;
+}
+bool LimitRead(int pin)
+{
+ int x;
+ x=limit[pin];
+ if(x==0) {
+ return false;
+ } else if(x==1) {
+ return true;
+ } else {
+ return false;
+ }
+}
+//関数、タイマーの宣言
+//////////////////////////////////
+//using namespace SWITCH;
+
+void SystemProcess(void)
+{
+ while(true) {
+ controller = XBEE::Controller::GetData();
+//____________________________
+ /*------------------------ HOW TO WRITE ------------------------/
+
+ ここにメインのプログラムを書く
+
+ ・コントローラから受け取ったデータをもとに動作のプログラムを書く
+ (コントローラのデータは controller-> で取る)
+
+ if(controller->Button.RIGHT) {
+ motor[TIRE_L].dir = FOR;
+ motor[TIRE_R].dir = BACK;
+ motor[TIRE_L].pwm = 12.3;
+ motor[TIRE_R].pwm = 12.3;
+ }
+
+ motor[0].dirは FOR (正転)
+ BACK (逆転)
+ BRAKE (ブレーキ)
+ FREE (フリー)
+
+ motor[0].pwmは 0.0(%) ~ 100.0(%)
+
+ controllerは XBee.hの構造体の中身
+
+ (AnalogL・Rを使いたかったら、頑張って考える or 聞いてください)
+
+ ・リミットスイッチの値をもとに動作のプログラムを書く
+
+ if(Switch::CheckPushed(ARM_L))
+ {
+ if(controller->Button.L)
+ {
+ motor[ARM].dir = FOR;
+ motor[ARM].pwm = 80.0;
+ }
+ if(motor[ARM].dir == BACK)
+ {
+ motor[ARM].dir = BRAKE;
+ }
+ }
+
+ →関数 Switch::CheckPushed の引数はリミットスイッチの名前 (limitSw[0]みたいな), 返り値はbool型 (true or false)
+
+ ・他にもやりたいことがあったら自由にどうぞ
+
+ ps.わからないことがあったら聞いてください
+
+ /------------------------ HOW TO WRITE ------------------------*/
+//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓
+
+ float precision; //精密モード変数
+ float s=Airtime.read();//エアータイム
+ float hometime=home.read();//ホームタイム（ご挨拶モード兼回路リセット）
+ float goaisatsu_time=0.2;//ご挨拶タイム
+ //////////////////////////////////////////////////////////////////////////////////
+ //ご挨拶モード
+
+ if(controller->Button.HOME) {
+ home.start();
+ homes=1;
+ }
+
+ if((hometime<=goaisatsu_time)&&(!(homes==0))) {
+ homes=2;
+ } else if(hometime<=2*goaisatsu_time) {
+ homes=1;
+ } else if(hometime<=3*goaisatsu_time) {
+ homes=2;
+ } else if(hometime<4*goaisatsu_time) {
+ homes=1;
+ } else if(hometime<5*goaisatsu_time) {
+ homes=0;
+ home.stop();
+ home.reset();
+ } else {
+ homes=0;
+ home.stop();
+ home.reset();
+ }
+
+ //ご挨拶モード
+ ////////////////////////////////////////////////////////////////////////////////////
+
+ ////////////////////////////////////////////////////////////////////////////////////
+ //精密モード
+ if(controller->Button.ZR) {
+ precision=0.5;
+ } else {
+ precision=1.0;
+ }
+ //精密モード
+ ////////////////////////////////////////////////////////////////////////////////////
+
+ ////////////////////////////////////////////////////////////////////////////////////
+ //アーム機構
+ //motor[ARM].pwm=100;
+
+ if(controller->Button.X) {
+ if(LimitRead(0)){
+ motor[ARM].dir = BACK;
+ motor[ARM].pwm = 100;
+ }else{
+ motor[ARM].dir = FREE;
+ motor[ARM].pwm = 100;
+ }
+ }else if(controller->Button.Y) {
+ motor[ARM].dir = FOR;
+ motor[ARM].pwm = 100;
+ }else{
+ motor[ARM].dir = FREE;
+ motor[ARM].pwm = 100;
+ }
+
+ //アーム機構
+ ////////////////////////////////////////////////////////////////////////////////////
+
+ ////////////////////////////////////////////////////////////////////////////////////
+ //十字キー足回り（宮川ver）
+
+ if(controller->Button.L) {
+ motor[TIRE_1].dir = FOR;
+ motor[TIRE_2].dir = FOR;
+ motor[TIRE_3].dir = FOR;
+ motor[TIRE_1].pwm = 100*precision;
+ motor[TIRE_2].pwm = 100*precision;
+ motor[TIRE_3].pwm = 100*precision;
+ } else if(controller->Button.R) {
+ motor[TIRE_1].dir = BACK;
+ motor[TIRE_2].dir = BACK;
+ motor[TIRE_3].dir = BACK;
+ motor[TIRE_1].pwm = 100*precision;
+ motor[TIRE_2].pwm = 100*precision;
+ motor[TIRE_3].pwm = 100*precision;
+ } else if(controller->Button.RIGHT) {
+ motor[TIRE_1].dir = BACK;
+ motor[TIRE_2].dir = FOR;
+ motor[TIRE_3].dir = FOR;
+ motor[TIRE_1].pwm = 100*precision;
+ motor[TIRE_2].pwm = 60*precision;
+ motor[TIRE_3].pwm = 60*precision;
+ } else if(controller->Button.LEFT) {
+ motor[TIRE_1].dir = FOR;
+ motor[TIRE_2].dir = BACK;
+ motor[TIRE_3].dir = BACK;
+ motor[TIRE_1].pwm = 100*precision;
+ motor[TIRE_2].pwm = 60*precision;
+ motor[TIRE_3].pwm = 60*precision;
+ } else if(controller->Button.DOWN) {
+ motor[TIRE_1].dir = BRAKE;
+ motor[TIRE_2].dir = FOR;
+ motor[TIRE_3].dir = BACK;
+ motor[TIRE_1].pwm = 100;
+ motor[TIRE_2].pwm = 100*precision;
+ motor[TIRE_3].pwm = 100*precision;
+ } else if(controller->Button.UP) {
+ motor[TIRE_1].dir = BRAKE;
+ motor[TIRE_2].dir = BACK;
+ motor[TIRE_3].dir = FOR;
+ motor[TIRE_1].pwm = 100;
+ motor[TIRE_2].pwm = 100*precision;
+ motor[TIRE_3].pwm = 100*precision;
+ } else if((controller->Button.UP) && (controller->Button.RIGHT)) {
+ motor[TIRE_1].dir = BACK;
+ motor[TIRE_2].dir = BRAKE;
+ motor[TIRE_3].dir = FOR;
+ motor[TIRE_1].pwm = 100*precision;
+ motor[TIRE_2].pwm = 100;
+ motor[TIRE_3].pwm = 100*precision;
+ } else if((controller->Button.UP) && (controller->Button.LEFT)) {
+ motor[TIRE_1].dir = FOR;
+ motor[TIRE_2].dir = BACK;
+ motor[TIRE_3].dir = BRAKE;
+ motor[TIRE_1].pwm = 100*precision;
+ motor[TIRE_2].pwm = 100*precision;
+ motor[TIRE_3].pwm = 100;
+ } else if((controller->Button.DOWN) && (controller->Button.RIGHT)) {
+ motor[TIRE_1].dir = BACK;
+ motor[TIRE_2].dir = FOR;
+ motor[TIRE_3].dir = BRAKE;
+ motor[TIRE_1].pwm = 100*precision;
+ motor[TIRE_2].pwm = 100*precision;
+ motor[TIRE_3].pwm = 100;
+ } else if((controller->Button.DOWN) && (controller->Button.LEFT)) {
+ motor[TIRE_1].dir = FOR;
+ motor[TIRE_2].dir = BRAKE;
+ motor[TIRE_3].dir = BACK;
+ motor[TIRE_1].pwm = 100*precision;
+ motor[TIRE_2].pwm = 100;
+ motor[TIRE_3].pwm = 100*precision;
+ } else if(homes==1) {
+ motor[TIRE_1].dir = FOR;
+ motor[TIRE_2].dir = FOR;
+ motor[TIRE_3].dir = FOR;
+ motor[TIRE_1].pwm = 70*precision;
+ motor[TIRE_2].pwm = 70*precision;
+ motor[TIRE_3].pwm = 70*precision;
+ } else if(homes==2) {
+ motor[TIRE_1].dir = BACK;
+ motor[TIRE_2].dir = BACK;
+ motor[TIRE_3].dir = BACK;
+ motor[TIRE_1].pwm = 70*precision;
+ motor[TIRE_2].pwm = 70*precision;
+ motor[TIRE_3].pwm = 70*precision;
+ } else {
+ motor[TIRE_1].dir=BRAKE;
+ motor[TIRE_2].dir=BRAKE;
+ motor[TIRE_3].dir=BRAKE;
+ motor[TIRE_1].pwm=100;
+ motor[TIRE_2].pwm=100;
+ motor[TIRE_3].pwm=100;
+ }
+ //十字キー足回り（みやがわver）
+ ///////////////////////////////////
+
+ ///////////////////////////////////
+ //課題１
+ if(controller->Button.A) {
+ AirOut(Air0,1);
+ } else if(controller->Button.B) {
+ AirOut(Air0,0);
+ }
+
+ if(controller->Button.ZL) {
+ Airtime.start();
+ AirOut(Air1,0);
+ }
+ if(s>1) {
+ Airtime.stop();
+ Airtime.reset();
+ AirOut(Air1,1);
+ }
+ //課題１
+ ///////////////////////////////////
+
+ /////////////////////////////////////////////////////////////////////////////////////
+ //アナログスティック足回り（新村ver）
+
+ conlx = controller->AnalogL.X;
+ conly = controller->AnalogL.Y;
+ conrx = controller->AnalogR.X;
+ conry = controller->AnalogR.Y;
+ if((!(controller->Button.RIGHT))&&(!(controller->Button.DOWN))&&
+ (!(controller->Button.LEFT))&&(!(controller->Button.UP))&&
+ (!(controller->Button.L))&&(!(controller->Button.R))&&
+ (!(controller->Button.HOME))) {
+ if(((conlx >6) && (conly <8)) ||((conrx >6)&&(conry <8))) {
+ motor[a].dir = BRAKE;
+ motor[b].dir = BRAKE;
+ motor[c].dir = BRAKE;
+ motor[a].pwm = 100.0;
+ motor[b].pwm = 100.0;
+ motor[c].pwm = 100.0;
+ }
+
+ if(a_array[conly][conlx]<0) {
+ motor[a].pwm = -1*a_array[conly][conlx]*precision;
+ motor[a].dir = FOR;
+ } else if(a_array[conly][conlx] > 0) {
+ motor[a].pwm = a_array[conly][conlx]*precision;
+ motor[a].dir = BACK;
+ } else if(a_array[conly][conlx] == 0) {
+ if(conry < 6) {
+ motor[a].dir = BACK;
+ motor[b].dir = BACK;
+ motor[c].dir = BACK;
+ motor[a].pwm = 100.0*precision;
+ motor[b].pwm = 100.0*precision;
+ motor[c].pwm = 100.0*precision;
+ } else if(conry > 8) {
+ motor[a].dir = FOR;
+ motor[b].dir = FOR;
+ motor[c].dir = FOR;
+ motor[a].pwm = 100.0*precision;
+ motor[b].pwm = 100.0*precision;
+ motor[c].pwm = 100.0*precision;
+ } else {
+ motor[a].dir = BRAKE;
+ motor[b].dir = BRAKE;
+ motor[c].dir = BRAKE;
+ motor[a].pwm = 100.0;
+ motor[b].pwm = 100.0;
+ motor[c].pwm = 100.0;
+ }
+ }
+
+ if(b_array[conly][conlx] <0 ) {
+ motor[b].pwm = -1*b_array[conly][conlx]*precision;
+ motor[b].dir = FOR;
+ } else if(b_array[conly][conlx] > 0) {
+ motor[b].pwm = b_array[conly][conlx]*precision;
+ motor[b].dir = BACK;
+ } else if(a_array[conly][conlx] == 0 ) {
+ if(conrx < 6 ) {
+ motor[a].dir = BACK;
+ motor[b].dir = BACK;
+ motor[c].dir = BACK;
+ motor[a].pwm = 100.0*precision;
+ motor[b].pwm = 100.0*precision;
+ motor[c].pwm = 100.0*precision;
+ } else if(conrx > 8) {
+ motor[a].dir = FOR*precision;
+ motor[b].dir = FOR*precision;
+ motor[c].dir = FOR*precision;
+ motor[a].pwm = 100.0*precision;
+ motor[b].pwm = 100.0*precision;
+ motor[c].pwm = 100.0*precision;
+ } else {
+ motor[a].dir = BRAKE;
+ motor[b].dir = BRAKE;
+ motor[c].dir = BRAKE;
+ motor[a].pwm = 100.0;
+ motor[b].pwm = 100.0;
+ motor[c].pwm = 100.0;
+ }
+ }
+
+
+ if(c_array[conly][conlx] <0 ) {
+ motor[c].pwm =-1*c_array[conly][conlx]*precision;
+ motor[c].dir = BACK;
+ } else if(c_array[conly][conlx] > 0) {
+ motor[c].pwm = c_array[conly][conlx]*precision;
+ motor[c].dir = FOR;
+ } else if(a_array[conly][conlx] == 0 ) {
+ if(conrx < 6 ) {
+ motor[a].dir = BACK;
+ motor[b].dir = BACK;
+ motor[c].dir = BACK;
+ motor[a].pwm = 100.0*precision;
+ motor[b].pwm = 100.0*precision;
+ motor[c].pwm = 100.0*precision;
+ } else if(conrx > 8) {
+ motor[a].dir = FOR;
+ motor[b].dir = FOR;
+ motor[c].dir = FOR;
+ motor[a].pwm = 100.0*precision;
+ motor[b].pwm = 100.0*precision;
+ motor[c].pwm = 100.0*precision;
+ } else {
+ motor[a].dir = BRAKE;
+ motor[b].dir = BRAKE;
+ motor[c].dir = BRAKE;
+ motor[a].pwm = 100.0;
+ motor[b].pwm = 100.0;
+ motor[c].pwm = 100.0;
+ }
+ //アナログスティック足回り（新村ver）
+ /////////////////////////////////////////////////////////////////////////////////////
+
+
+
+
+
+
+ }
+ }
+
+
+
+//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑
+
+
+ MOTOR::Motor::Update(motor);
+ }
+ }
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/System/Process/Process.h Fri May 03 11:47:27 2019 +0000 @@ -0,0 +1,7 @@ +#ifndef PROCESS_H_ +#define PROCESS_H_ + +void SystemProcess(void); + +#endif +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri May 03 11:47:27 2019 +0000
@@ -0,0 +1,10 @@
+#include "mbed.h"
+
+#include "System/Initialize/Initialize.h"
+#include "System/Process/Process.h"
+
+int main(void) {
+ SystemInitialize();
+ SystemProcess();
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Fri May 03 11:47:27 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file