WRS2020用にメカナム台車をROS化
Dependencies: mbed PID2 JY901 QuaternionMath ros_lib_melodic i2cmaster WRS2020_mecanum_node
Dependents: WRS2020_mecanum_node
motor_controller.h
- Committer:
- sgrsn
- Date:
- 2020-11-02
- Revision:
- 0:c177452db984
File content as of revision 0:c177452db984:
#include "mbed.h" #include "device.h" int addr[MOTOR_NUM] = {IIC_ADDR1, IIC_ADDR2, IIC_ADDR3, IIC_ADDR4}; int controlMotor(int ch, int frequency) { int dir = COAST; int size = 4; if(ch < 0 || ch > 3) { //channel error return 0; } else { // オドメトリ用に周波数を保存 my_odometry.wheel_frequency[ch] = frequency; if(frequency > 0) { dir = CW; } else if(frequency < 0) { dir = CCW; frequency = -frequency; } else { dir = BRAKE; } // 周波数制限 脱調を防ぐ if(frequency > MaxFrequency) frequency = MaxFrequency; //else if(frequency < MinFrequency) frequency = MinFrequency; master.writeSomeData(addr[ch], PWM_FREQUENCY, frequency, size); master.writeSomeData(addr[ch], MOTOR_DIR, dir, size); return frequency; } } void coastAllMotor() { for(int i = 0; i < MOTOR_NUM; i++) { // オドメトリ用に周波数を保存 my_odometry.wheel_frequency[i] = 0; master.writeSomeData(addr[i], MOTOR_DIR, COAST, 4); } }