hidaka sato / Mbed 2 deprecated WRS2020_mecanum_node

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?

UserRevisionLine numberNew 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 }