Revision 2:32d2cd7d744b, committed 2020-01-23

Comitter:

M_souta

Date:

Thu Jan 23 09:16:38 2020 +0000

Parent:

1:5b0303768126

Child:

3:8790e69f8c59

Commit message:

lll

Changed in this revision

--- a/MCP/MCP.cpp	Tue Jan 21 11:10:33 2020 +0000
+++ b/MCP/MCP.cpp	Thu Jan 23 09:16:38 2020 +0000
@@ -6,7 +6,7 @@
     :i2c(sda, scl), mcp(i2c, device_address) 
 {
     _iodir_data.all = 0xffff;
-    _pull_data.all  = 0x0000;
+    _pull_data.all  = 0xffff;
     _read_data.all  = 0x0000;
     _write_data.all = 0x0000;
 }
@@ -37,14 +37,20 @@
     return (_read_data.all >> pin) & 0x01;
 }
         
-void MCP::Update(void)
-{
+void MCP::Update(void) {
+    char data[2] = {GPPUA, 0xff};
+    int lost = i2c.write(0x40, data, 2);
+    if(lost) {
+        i2c.start();
+    }
     mcp.direction(PORT_A, _iodir_data.port.port_A);
     mcp.direction(PORT_B, _iodir_data.port.port_B);
     mcp.configurePullUps(PORT_A, _pull_data.port.port_A);
     mcp.configurePullUps(PORT_B, _pull_data.port.port_B);
     _read_data.port.port_A = mcp.read(PORT_A);
     _read_data.port.port_B = mcp.read(PORT_B);
-   // mcp.write(PORT_A, _write_data.port_A);
-   // mcp.write(PORT_B, _write_data.port_B);
+    mcp.write(PORT_A, _write_data.port.port_A);
+    mcp.write(PORT_B, _write_data.port.port_B);
+    
 }
+    
\ No newline at end of file

--- a/MCP/MCP.h	Tue Jan 21 11:10:33 2020 +0000
+++ b/MCP/MCP.h	Thu Jan 23 09:16:38 2020 +0000
@@ -9,27 +9,22 @@
 
 #define MCP_ADDRESS 0x40
 
-
-
-typedef enum {
-    a0 = 0,
-    a1 = 1,
-    a2 = 2,
-    a3 = 3,
-    a4 = 4,
-    a5 = 5,
-    a6 = 6,
-    a7 = 7,
-    b0 = 8,
-    b1 = 9,
-    b2 = 10,
-    b3 = 11,
-    b4 = 12,
-    b5 = 13,
-    b6 = 14,
-    b7 = 15,
-}pin_name;
-
+#define GPA0 0
+#define GPA1 1
+#define GPA2 2
+#define GPA3 3
+#define GPA4 4
+#define GPA5 5
+#define GPA6 6
+#define GPA7 7
+#define GPB0 8
+#define GPB1 9
+#define GPB2 10
+#define GPB3 11
+#define GPB4 12
+#define GPB5 13
+#define GPB6 14
+#define GPB7 15
 
 typedef enum {
     OUTPUT,
@@ -63,8 +58,6 @@
         void Initialize(void);
         // MCP register update and read new data
         void Update(void);
-        // i2c error check
-        bool ErrorOccurred(void);
         
     private:
         I2C i2c;
@@ -79,6 +72,8 @@
         
         void _Write(uint8_t address, uint8_t data);
         void _Read(uint8_t address, uint8_t data);
+        
+        void I2Crestart(void);
     
 };
 

--- a/Motor/Motor.cpp	Tue Jan 21 11:10:33 2020 +0000
+++ b/Motor/Motor.cpp	Thu Jan 23 09:16:38 2020 +0000
@@ -1,30 +1,86 @@
 #include "Motor.h"
 
+#include <stdint.h>
 #include "mbed.h"
 
-MOTOR::MOTOR(PinName D1, PinName D2, PinName pwm)
-    :D1_(D1), D2_(D2), pwm_(pwm)
-{
-    D1_.write(0);
-    D2_.write(0);
-    pwm_.period_us(50);  // 20kHz
-    pwm_.write(0);
-}
-
-void MOTOR::Dir(dire mode, uint8_t pwm)
+namespace MOTOR
 {
-    D1_.write((char)mode & 0x01);
-    D2_.write((char)mode & 0x02);
-    pwm_.write((float)(pwm / 255.0));
-}
+    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;
+        }
 
-void MOTOR::Dir(dire mode)
-{
-    D1_.write((char)mode & 0x01);
-    D2_.write((char)mode & 0x02);
+        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;
+    }
+    
+    float Motor::SetPWM(float pwm) {
+        if(pwm > 0.0)       return pwm;
+        else if(pwm < 0.0)  return -pwm;
+        else                return 80;
+    }
+    
+    
 }
-
-void MOTOR::PWM(uint8_t pwm)
-{
-    pwm_.write((float)(pwm / 255.0));
-}
\ No newline at end of file

--- a/Motor/Motor.h	Tue Jan 21 11:10:33 2020 +0000
+++ b/Motor/Motor.h	Thu Jan 23 09:16:38 2020 +0000
@@ -1,40 +1,76 @@
-#ifndef  MOTOR_H_
-#define  MOTOR_H_
+#ifndef MOTOR_H_
+#define MOTOR_H_
+
+#include <stdint.h>
 
-#include "mbed.h"
+//#define USE_MOTOR0_SENSOR
+namespace MOTOR
+{
+    #define FREE    0
+    #define BACK    1
+    #define FOR     2
+    #define BRAKE   3
+
+    #define MOUNTING_MOTOR_NUM 5
 
-typedef enum dire {
-    FREE  = 0,
-    FOR   = 1,
-    BACK  = 2,
-    BRAKE = 3,
-}dire;
+    #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
 
-class MOTOR {
-    public:
-        // 
-        MOTOR(PinName D1, PinName D2, PinName pwm);
-        /* direction
-            FREE
-            FOR
-            BACK
-            BRAKE
-        */
-        /* pwm
-            pwm is 0 ~ 255(%)
-        */
-        // durection and pwm set
-        void Dir(dire mode, uint8_t pwm);
-        // direction set
-        void Dir(dire mode);
-        // pwm set
-        void PWM(uint8_t pwm);
-        
-    private:
-        DigitalOut D1_;
-        DigitalOut D2_;
-        PwmOut     pwm_;
-};
+    #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
 
-#endif //MOTOR_H_
+    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);
+            static float SetPWM(float pwm);
+    };
+}
+
+#endif

--- a/main.cpp	Tue Jan 21 11:10:33 2020 +0000
+++ b/main.cpp	Thu Jan 23 09:16:38 2020 +0000
@@ -7,27 +7,44 @@
 #define SCL PB_6
 #define MCP_ADDRESS 0x40
 
+using namespace MOTOR
+
 MCP MCP(SDA, SCL, MCP_ADDRESS);
 
 Serial pc(USBTX, USBRX);
 
 XBEE::ControllerData *controller;
+MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
 
 int main() {
     
-    for(int i = 0; i < 16; i++) {
-        MCP.PinMode(i, INPUT_PULLUP);
-    }
+    __enable_irq();
+    
+    MOTOR::Motor::Initialize();
+    
+    MCP.PinMode(7, INPUT_PULLUP);
+    MCP.PinMode(6, OUTPUT);
+    
     
     while(1) {
         controller = XBEE::Controller::GetData();
+        
         MCP.Update();
         
+        int a;
+        
         /* write ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ */
-        //for(int i = 0; i < 16; i++) {
-            int a = MCP.Read(7);
+        for(int i = 0; i < 16; i++) {
+            a = MCP.Read(i);
             pc.printf("%d",a);
-        //}
+        }
         pc.printf("\n\r");
+        
+        int b = MCP.Read(7);
+        
+        MCP.Write(6, b);
+        
+        //
+        MOTOR::Motor::Update(motor);
     }
 }