WRS2020用にメカナム台車をROS化
Dependencies: mbed PID2 JY901 QuaternionMath ros_lib_melodic i2cmaster WRS2020_mecanum_node
Dependents: WRS2020_mecanum_node
motor_controller.h@5:17ede03f1fa5, 2021-02-23 (annotated)
- Committer:
- sgrsn
- Date:
- Tue Feb 23 10:22:26 2021 +0000
- Revision:
- 5:17ede03f1fa5
- Parent:
- 0:c177452db984
Lateset commit;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
sgrsn | 0:c177452db984 | 1 | #include "mbed.h" |
sgrsn | 0:c177452db984 | 2 | #include "device.h" |
sgrsn | 0:c177452db984 | 3 | |
sgrsn | 0:c177452db984 | 4 | int addr[MOTOR_NUM] = {IIC_ADDR1, IIC_ADDR2, IIC_ADDR3, IIC_ADDR4}; |
sgrsn | 0:c177452db984 | 5 | |
sgrsn | 0:c177452db984 | 6 | int controlMotor(int ch, int frequency) |
sgrsn | 0:c177452db984 | 7 | { |
sgrsn | 0:c177452db984 | 8 | int dir = COAST; |
sgrsn | 0:c177452db984 | 9 | int size = 4; |
sgrsn | 0:c177452db984 | 10 | if(ch < 0 || ch > 3) |
sgrsn | 0:c177452db984 | 11 | { |
sgrsn | 0:c177452db984 | 12 | //channel error |
sgrsn | 0:c177452db984 | 13 | return 0; |
sgrsn | 0:c177452db984 | 14 | } |
sgrsn | 0:c177452db984 | 15 | else |
sgrsn | 0:c177452db984 | 16 | { |
sgrsn | 0:c177452db984 | 17 | // オドメトリ用に周波数を保存 |
sgrsn | 0:c177452db984 | 18 | my_odometry.wheel_frequency[ch] = frequency; |
sgrsn | 0:c177452db984 | 19 | if(frequency > 0) |
sgrsn | 0:c177452db984 | 20 | { |
sgrsn | 0:c177452db984 | 21 | dir = CW; |
sgrsn | 0:c177452db984 | 22 | } |
sgrsn | 0:c177452db984 | 23 | else if(frequency < 0) |
sgrsn | 0:c177452db984 | 24 | { |
sgrsn | 0:c177452db984 | 25 | dir = CCW; |
sgrsn | 0:c177452db984 | 26 | frequency = -frequency; |
sgrsn | 0:c177452db984 | 27 | } |
sgrsn | 0:c177452db984 | 28 | else |
sgrsn | 0:c177452db984 | 29 | { |
sgrsn | 0:c177452db984 | 30 | dir = BRAKE; |
sgrsn | 0:c177452db984 | 31 | } |
sgrsn | 0:c177452db984 | 32 | // 周波数制限 脱調を防ぐ |
sgrsn | 0:c177452db984 | 33 | if(frequency > MaxFrequency) frequency = MaxFrequency; |
sgrsn | 0:c177452db984 | 34 | //else if(frequency < MinFrequency) frequency = MinFrequency; |
sgrsn | 0:c177452db984 | 35 | |
sgrsn | 0:c177452db984 | 36 | master.writeSomeData(addr[ch], PWM_FREQUENCY, frequency, size); |
sgrsn | 0:c177452db984 | 37 | master.writeSomeData(addr[ch], MOTOR_DIR, dir, size); |
sgrsn | 0:c177452db984 | 38 | |
sgrsn | 0:c177452db984 | 39 | return frequency; |
sgrsn | 0:c177452db984 | 40 | } |
sgrsn | 0:c177452db984 | 41 | } |
sgrsn | 0:c177452db984 | 42 | |
sgrsn | 0:c177452db984 | 43 | |
sgrsn | 0:c177452db984 | 44 | void coastAllMotor() |
sgrsn | 0:c177452db984 | 45 | { |
sgrsn | 0:c177452db984 | 46 | for(int i = 0; i < MOTOR_NUM; i++) |
sgrsn | 0:c177452db984 | 47 | { |
sgrsn | 0:c177452db984 | 48 | // オドメトリ用に周波数を保存 |
sgrsn | 0:c177452db984 | 49 | my_odometry.wheel_frequency[i] = 0; |
sgrsn | 0:c177452db984 | 50 | master.writeSomeData(addr[i], MOTOR_DIR, COAST, 4); |
sgrsn | 0:c177452db984 | 51 | } |
sgrsn | 0:c177452db984 | 52 | } |