hidaka sato / MecanumInterface

MecanumInterface library to use MecanumController library

Dependents:   WRS2021_mecanum_driver

mecanum_interface.hpp@1:bc9347a4aa27, 2021-08-25 (annotated)

Committer:
sgrsn
Date:
Wed Aug 25 08:51:38 2021 +0000
Revision:
1:bc9347a4aa27
Parent:
0:63a9a160432b 
Fix parameter

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sgrsn 0:63a9a160432b 1 #ifndef MECANUM_INTERFACE_HPP
sgrsn 0:63a9a160432b 2 #define MECANUM_INTERFACE_HPP
sgrsn 0:63a9a160432b 3
sgrsn 0:63a9a160432b 4 #include "i2cmaster.h"
sgrsn 0:63a9a160432b 5
sgrsn 0:63a9a160432b 6 #define MOTOR_NUM 4
sgrsn 0:63a9a160432b 7 #define IIC_ADDR1 0xB0
sgrsn 0:63a9a160432b 8 #define IIC_ADDR2 0xC0
sgrsn 0:63a9a160432b 9 #define IIC_ADDR3 0xD0
sgrsn 0:63a9a160432b 10 #define IIC_ADDR4 0xE0
sgrsn 0:63a9a160432b 11
sgrsn 1:bc9347a4aa27 12 // AR46SAK-PS50-1
sgrsn 1:bc9347a4aa27 13 // Experimentally determined maximum frequency
sgrsn 1:bc9347a4aa27 14 #define MaxFrequency 25000
sgrsn 1:bc9347a4aa27 15 #define MinFrequency 0
sgrsn 0:63a9a160432b 16
sgrsn 0:63a9a160432b 17 typedef enum
sgrsn 0:63a9a160432b 18 {
sgrsn 0:63a9a160432b 19 COAST = 0,
sgrsn 0:63a9a160432b 20 BRAKE = 1,
sgrsn 0:63a9a160432b 21 CW = 2,
sgrsn 0:63a9a160432b 22 CCW = 3
sgrsn 0:63a9a160432b 23 }MotorState;
sgrsn 0:63a9a160432b 24
sgrsn 0:63a9a160432b 25 #define WHO_AM_I 0x00
sgrsn 0:63a9a160432b 26 #define MY_IIC_ADDR 0x01
sgrsn 0:63a9a160432b 27 #define MOTOR_DIR 0x04
sgrsn 0:63a9a160432b 28 #define PWM_FREQUENCY 0x05
sgrsn 0:63a9a160432b 29
sgrsn 0:63a9a160432b 30 class MecanumInterface
sgrsn 0:63a9a160432b 31 {
sgrsn 0:63a9a160432b 32 public:
sgrsn 0:63a9a160432b 33 MecanumInterface()
sgrsn 0:63a9a160432b 34 {
sgrsn 0:63a9a160432b 35 }
sgrsn 0:63a9a160432b 36
sgrsn 0:63a9a160432b 37 int ControlMotor(int ch, int frequency)
sgrsn 0:63a9a160432b 38 {
sgrsn 0:63a9a160432b 39 int dir = COAST;
sgrsn 0:63a9a160432b 40 int cmd_frequency = 0;
sgrsn 0:63a9a160432b 41 if(ch < 0 || ch > 3)
sgrsn 0:63a9a160432b 42 {
sgrsn 0:63a9a160432b 43 //channel error
sgrsn 0:63a9a160432b 44 return 0;
sgrsn 0:63a9a160432b 45 }
sgrsn 0:63a9a160432b 46 else
sgrsn 0:63a9a160432b 47 {
sgrsn 0:63a9a160432b 48 if(frequency > 0)
sgrsn 0:63a9a160432b 49 {
sgrsn 0:63a9a160432b 50 dir = CW;
sgrsn 0:63a9a160432b 51 cmd_frequency = frequency;
sgrsn 0:63a9a160432b 52 }
sgrsn 0:63a9a160432b 53 else if(frequency < 0)
sgrsn 0:63a9a160432b 54 {
sgrsn 0:63a9a160432b 55 dir = CCW;
sgrsn 0:63a9a160432b 56 cmd_frequency = -frequency;
sgrsn 0:63a9a160432b 57 }
sgrsn 0:63a9a160432b 58 else
sgrsn 0:63a9a160432b 59 {
sgrsn 0:63a9a160432b 60 dir = BRAKE;
sgrsn 0:63a9a160432b 61 }
sgrsn 0:63a9a160432b 62 // 周波数制限 脱調を防ぐ
sgrsn 1:bc9347a4aa27 63 if(cmd_frequency > MaxFrequency)
sgrsn 1:bc9347a4aa27 64 {
sgrsn 1:bc9347a4aa27 65 cmd_frequency = MaxFrequency;
sgrsn 1:bc9347a4aa27 66 if(dir == CW) frequency = MaxFrequency;
sgrsn 1:bc9347a4aa27 67 else if(dir == CCW) frequency = -MaxFrequency;
sgrsn 1:bc9347a4aa27 68 }
sgrsn 0:63a9a160432b 69 //else if(cmd_frequency < MinFrequency) cmd_frequency = MinFrequency;
sgrsn 0:63a9a160432b 70
sgrsn 0:63a9a160432b 71 CommandMotor(ch, dir, cmd_frequency);
sgrsn 0:63a9a160432b 72
sgrsn 0:63a9a160432b 73 return frequency;
sgrsn 0:63a9a160432b 74 }
sgrsn 0:63a9a160432b 75 }
sgrsn 0:63a9a160432b 76
sgrsn 0:63a9a160432b 77 virtual void CommandMotor(int ch, int dir, int frequency)
sgrsn 0:63a9a160432b 78 {
sgrsn 0:63a9a160432b 79 }
sgrsn 0:63a9a160432b 80 };
sgrsn 0:63a9a160432b 81
sgrsn 0:63a9a160432b 82 class MecanumI2C : public MecanumInterface
sgrsn 0:63a9a160432b 83 {
sgrsn 0:63a9a160432b 84 public:
sgrsn 0:63a9a160432b 85 MecanumI2C(i2c *device)
sgrsn 0:63a9a160432b 86 {
sgrsn 0:63a9a160432b 87 myi2c_ = device;
sgrsn 0:63a9a160432b 88 }
sgrsn 0:63a9a160432b 89
sgrsn 0:63a9a160432b 90 void CommandMotor(int ch, int dir, int frequency)
sgrsn 0:63a9a160432b 91 {
sgrsn 0:63a9a160432b 92 int size = 4;
sgrsn 0:63a9a160432b 93 int addr[MOTOR_NUM] = {IIC_ADDR1, IIC_ADDR2, IIC_ADDR3, IIC_ADDR4};
sgrsn 0:63a9a160432b 94 myi2c_ -> writeSomeData(addr[ch], PWM_FREQUENCY, frequency, size);
sgrsn 0:63a9a160432b 95 myi2c_ -> writeSomeData(addr[ch], MOTOR_DIR, dir, size);
sgrsn 0:63a9a160432b 96 }
sgrsn 0:63a9a160432b 97
sgrsn 0:63a9a160432b 98 private:
sgrsn 0:63a9a160432b 99 i2c* myi2c_;
sgrsn 0:63a9a160432b 100 };
sgrsn 0:63a9a160432b 101
sgrsn 0:63a9a160432b 102 #endif