main.cpp

00001 #include "mbed.h"
00002 #include "Motor.h"
00003 #include "MCP.h"
00004 #include "XBee.h"
00005 #include <stdint.h>
00006 
00007 #define SDA PB_7
00008 #define SCL PB_6
00009 #define MCP_ADDRESS 0x40
00010 
00011 MCP MCP(SDA, SCL, MCP_ADDRESS);
00012 XBEE::ControllerData *controller;
00013 MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
00014 
00015 /* ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ */
00016 //ピン名変更
00017 #define FL 0
00018 #define FR 1
00019 #define BL 2
00020 #define BR 3
00021 #define CC 4
00022 #define Lim0 0
00023 #define Lim1 1
00024 #define AIR0 2
00025 #define AIR1 3
00026 #define AIR2 4
00027 #define SL0 5
00028 #define SL1 6
00029 
00030 //リミットスイッチ　読み取る
00031 int g = MCP.Read(0);
00032 const int omni[15][15] = {
00033     {    0,     5,    21,     47,     83,    130,    187,    255,    255,    255,    255,    255,    255,    255,    255 },
00034     {   -5,     0,     5,     21,     47,     83,    130,    187,    193,    208,    234,    255,    255,    255,    255 },
00035     {  -21,    -5,     0,      5,     21,     47,     83,    130,    135,    151,    177,    213,    255,    255,    255 },
00036     {  -47,   -21,     5,      0,      5,     21,     47,     83,     88,    104,    130,    167,    213,    255,    255 },
00037     {  -83,   -47,    -21,     5,      0,      5,     21,     47,     52,     68,     94,    130,    177,    234,    255 },
00038     { -130,   -83,    -47,    -21,     5,      0,      5,     21,     26,     42,     68,    104,    151,    208,    255 },
00039     { -187,  -130,    -83,    -47,    -21,    -5,      0,      5,     10,     26,     52,     88,    135,    193,    255 },
00040     { -255,  -187,   -130,    -83,    -47,    -21,    -5,      0,      5,     21,     47,     83,    130,    187,    255 },
00041     { -255,  -193,   -135,    -88,    -52,    -26,    -10,    -5,      0,      5,     21,     47,     83,    130,    187 },
00042     { -255,  -208,   -151,   -104,    -68,    -42,    -26,    -21,    -5,      0,      5,     21,     47,     83,    130 },
00043     { -255,  -234,   -177,   -130,    -94,    -68,    -52,    -47,    -21,    -7,      0,      7,     21,     47,     83 },
00044     { -255,  -255,   -213,   -167,   -130,   -104,    -88,    -83,    -47,    -21,    -5,      0,      5,     21,     47 },
00045     { -255,  -255,   -255,   -213,   -177,   -151,   -135,   -130,    -83,    -47,    -21,    -5,      0,      5,     21 },
00046     { -255,  -255,   -255,   -255,   -234,   -208,   -193,   -187,   -130,    -83,    -47,    -21,    -5,      0,      5 },
00047     { -255,  -255,   -255,   -255,   -255,   -255,   -255,   -255,   -187,   -130,    -83,    -47,   -21,     -5,      0 }
00048 };
00049 
00050 const int curve[15] = { -204, -150, -104, -66, -38, -17, -4, 0, 4, 17, 38, 66, 104, 150, 204 };
00051 
00052 //uint8_t SetStatus(int pwmVal);
00053 //uint8_t SetPWM(int pwmVal);
00054 /* ↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑ */
00055 
00056 int main()
00057 {
00058 
00059     XBEE::Controller::Initialize();
00060     MOTOR::Motor::Initialize();
00061 
00062     __enable_irq();
00063 
00064     /* ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ */
00065 
00066 
00067 
00068     /* ↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑ */
00069 
00070     while(1) {
00071 
00072         controller = XBEE::Controller::GetData();
00073         MCP.Update();
00074 
00075         /* ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ */
00076         //足回り
00077         //左スティック　向き + 右スティック　向き
00078         motor[FL].dir = MOTOR::Motor::SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X]  + curve[controller->AnalogR.X]);
00079         motor[FR].dir = MOTOR::Motor::SetStatus(omni[14-controller->AnalogL.Y][controller->AnalogL.X]  +curve[controller->AnalogR.X]);
00080         motor[BL].dir=MOTOR::Motor::SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X]  +  curve[controller->AnalogR.X]);
00081         motor[BR].dir=MOTOR::Motor::SetStatus(omni[14-controller->AnalogL.Y][controller->AnalogL.X]  +  curve[14-controller->AnalogR.X]);
00082         //左スティック　PWM  +  右スティック　PWM
00083         motor[FL].pwm=MOTOR::Motor::SetPWM(omni[controller->AnalogL.Y][controller->AnalogL.X]  + curve[controller->AnalogR.X]);
00084         motor[FR].pwm=MOTOR::Motor::SetPWM(omni[14-controller->AnalogL.Y][controller->AnalogL.X]  +  curve[14-controller->AnalogR.X]);
00085         motor[BR].pwm=MOTOR::Motor::SetPWM(omni[controller->AnalogL.Y][controller->AnalogL.X]  +  curve[controller->AnalogR.X]);
00086         motor[BL].pwm=MOTOR::Motor::SetPWM(omni[14-controller->AnalogL.Y][controller->AnalogL.X]  + curve[14-controller->AnalogR.X]);
00087         //回収
00088         if(controller-> Button.A) {
00089             motor[CC].pwm=50;
00090             motor[CC].dir = FOR;
00091         } 
00092         else if(controller-> Button.B) {
00093             motor[CC].pwm=50;
00094             motor[CC].dir=BACK;
00095         } 
00096         else {
00097             motor[CC].pwm=100;
00098             motor[CC].dir=BRAKE;
00099         }
00100         //リミットスイッチのプログラム
00101         if(Lim0==1 || Lim1==1) {
00102             motor[CC].pwm=100;
00103             motor[CC].dir=BRAKE;
00104         }
00105 
00106         //投射
00107         if(controller-> Button.Y) {
00108             MCP.Write(AIR2,1);
00109         } 
00110         else {
00111             MCP.Write(AIR2,0);
00112         }
00113         //スロープ
00114         if(controller-> Button.L) {
00115             MCP.Write(AIR1,1);
00116         } 
00117         else {
00118             MCP.Write(AIR1,0);
00119         }
00120 
00121             if(controller-> Button.ZL) {
00122                 MCP.Write(SL1,1);
00123             } 
00124             else {
00125                 MCP.Write(SL1,0);
00126             }
00127 
00128             //ストックボックス
00129             if(controller-> Button.R) {
00130                 MCP.Write(AIR0,1);
00131             } 
00132             else {
00133                 MCP.Write(AIR0,0);
00134             }
00135             
00136             if(controller-> Button.ZR) {
00137                 MCP.Write(SL0,1);
00138             } 
00139             else {
00140                 MCP.Write(SL0,0);
00141             }
00142 
00143             /* ↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑ */
00144 
00145             MOTOR::Motor::Update(motor);
00146         }
00147     }