Revision 5:4b841637618e, committed 2020-01-29

Comitter:

tttko

Date:

Wed Jan 29 07:03:39 2020 +0000

Parent:

4:15547d07f8e5

Commit message:

aa

Changed in this revision

--- a/main.cpp	Sat Jan 25 00:40:35 2020 +0000
+++ b/main.cpp	Wed Jan 29 07:03:39 2020 +0000
@@ -4,7 +4,7 @@
 #include "XBee.h"
 #include <stdint.h>
 
-#define SDA PB_7 
+#define SDA PB_7
 #define SCL PB_6
 #define MCP_ADDRESS 0x40
 
@@ -13,35 +13,135 @@
 MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
 
 /* ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ */
-    
-    
-    
+//ピン名変更
+#define FL 0
+#define FR 1
+#define BL 2
+#define BR 3
+#define CC 4
+#define Lim0 0
+#define Lim1 1
+#define AIR0 2
+#define AIR1 3
+#define AIR2 4
+#define SL0 5
+#define SL1 6
+
+//リミットスイッチ　読み取る
+int g = MCP.Read(0);
+const int omni[15][15] = {
+    {    0,     5,    21,     47,     83,    130,    187,    255,    255,    255,    255,    255,    255,    255,    255 },
+    {   -5,     0,     5,     21,     47,     83,    130,    187,    193,    208,    234,    255,    255,    255,    255 },
+    {  -21,    -5,     0,      5,     21,     47,     83,    130,    135,    151,    177,    213,    255,    255,    255 },
+    {  -47,   -21,     5,      0,      5,     21,     47,     83,     88,    104,    130,    167,    213,    255,    255 },
+    {  -83,   -47,    -21,     5,      0,      5,     21,     47,     52,     68,     94,    130,    177,    234,    255 },
+    { -130,   -83,    -47,    -21,     5,      0,      5,     21,     26,     42,     68,    104,    151,    208,    255 },
+    { -187,  -130,    -83,    -47,    -21,    -5,      0,      5,     10,     26,     52,     88,    135,    193,    255 },
+    { -255,  -187,   -130,    -83,    -47,    -21,    -5,      0,      5,     21,     47,     83,    130,    187,    255 },
+    { -255,  -193,   -135,    -88,    -52,    -26,    -10,    -5,      0,      5,     21,     47,     83,    130,    187 },
+    { -255,  -208,   -151,   -104,    -68,    -42,    -26,    -21,    -5,      0,      5,     21,     47,     83,    130 },
+    { -255,  -234,   -177,   -130,    -94,    -68,    -52,    -47,    -21,    -7,      0,      7,     21,     47,     83 },
+    { -255,  -255,   -213,   -167,   -130,   -104,    -88,    -83,    -47,    -21,    -5,      0,      5,     21,     47 },
+    { -255,  -255,   -255,   -213,   -177,   -151,   -135,   -130,    -83,    -47,    -21,    -5,      0,      5,     21 },
+    { -255,  -255,   -255,   -255,   -234,   -208,   -193,   -187,   -130,    -83,    -47,    -21,    -5,      0,      5 },
+    { -255,  -255,   -255,   -255,   -255,   -255,   -255,   -255,   -187,   -130,    -83,    -47,   -21,     -5,      0 }
+};
+
+const int curve[15] = { -204, -150, -104, -66, -38, -17, -4, 0, 4, 17, 38, 66, 104, 150, 204 };
+
+//uint8_t SetStatus(int pwmVal);
+//uint8_t SetPWM(int pwmVal);
 /* ↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑ */
 
-int main() {
-    
+int main()
+{
+
     XBEE::Controller::Initialize();
     MOTOR::Motor::Initialize();
-    
-     __enable_irq();
-    
+
+    __enable_irq();
+
     /* ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ */
-    
-    
-    
+
+
+
     /* ↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑ */
-    
+
     while(1) {
-        
+
         controller = XBEE::Controller::GetData();
         MCP.Update();
-        
+
         /* ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ */
-        
-        
-        
-        /* ↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑ */
-        
-        MOTOR::Motor::Update(motor);
+        //足回り
+        //左スティック　向き + 右スティック　向き
+        motor[FL].dir = MOTOR::Motor::SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X]  + curve[controller->AnalogR.X]);
+        motor[FR].dir = MOTOR::Motor::SetStatus(omni[14-controller->AnalogL.Y][controller->AnalogL.X]  +curve[controller->AnalogR.X]);
+        motor[BL].dir=MOTOR::Motor::SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X]  +  curve[controller->AnalogR.X]);
+        motor[BR].dir=MOTOR::Motor::SetStatus(omni[14-controller->AnalogL.Y][controller->AnalogL.X]  +  curve[14-controller->AnalogR.X]);
+        //左スティック　PWM  +  右スティック　PWM
+        motor[FL].pwm=MOTOR::Motor::SetPWM(omni[controller->AnalogL.Y][controller->AnalogL.X]  + curve[controller->AnalogR.X]);
+        motor[FR].pwm=MOTOR::Motor::SetPWM(omni[14-controller->AnalogL.Y][controller->AnalogL.X]  +  curve[14-controller->AnalogR.X]);
+        motor[BR].pwm=MOTOR::Motor::SetPWM(omni[controller->AnalogL.Y][controller->AnalogL.X]  +  curve[controller->AnalogR.X]);
+        motor[BL].pwm=MOTOR::Motor::SetPWM(omni[14-controller->AnalogL.Y][controller->AnalogL.X]  + curve[14-controller->AnalogR.X]);
+        //回収
+        if(controller-> Button.A) {
+            motor[CC].pwm=50;
+            motor[CC].dir = FOR;
+        } 
+        else if(controller-> Button.B) {
+            motor[CC].pwm=50;
+            motor[CC].dir=BACK;
+        } 
+        else {
+            motor[CC].pwm=100;
+            motor[CC].dir=BRAKE;
+        }
+        //リミットスイッチのプログラム
+        if(Lim0==1 || Lim1==1) {
+            motor[CC].pwm=100;
+            motor[CC].dir=BRAKE;
+        }
+
+        //投射
+        if(controller-> Button.Y) {
+            MCP.Write(AIR2,1);
+        } 
+        else {
+            MCP.Write(AIR2,0);
+        }
+        //スロープ
+        if(controller-> Button.L) {
+            MCP.Write(AIR1,1);
+        } 
+        else {
+            MCP.Write(AIR1,0);
+        }
+
+            if(controller-> Button.ZL) {
+                MCP.Write(SL1,1);
+            } 
+            else {
+                MCP.Write(SL1,0);
+            }
+
+            //ストックボックス
+            if(controller-> Button.R) {
+                MCP.Write(AIR0,1);
+            } 
+            else {
+                MCP.Write(AIR0,0);
+            }
+            
+            if(controller-> Button.ZR) {
+                MCP.Write(SL0,1);
+            } 
+            else {
+                MCP.Write(SL0,0);
+            }
+
+            /* ↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑ */
+
+            MOTOR::Motor::Update(motor);
+        }
     }
-}