ken fuji / mecanum2017

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

mecanum2017.cpp@0:d4e9f39c6086, 2017-08-17 (annotated)

Committer:
fujikenac
Date:
Thu Aug 17 03:47:38 2017 +0000
Revision:
0:d4e9f39c6086
Child:
1:cc63373e84ec
ver1.0

Who changed what in which revision?

UserRevisionLine numberNew contents of line
fujikenac 0:d4e9f39c6086 1 #include "mbed.h"
fujikenac 0:d4e9f39c6086 2 #include "T_motor.h"
fujikenac 0:d4e9f39c6086 3 #include "mecanum2017.h"
fujikenac 0:d4e9f39c6086 4
fujikenac 0:d4e9f39c6086 5 mecanum2017::mecanum2017(I2C& i2c_, char addr1, char addr2, char addr3, char addr4, int phase)
fujikenac 0:d4e9f39c6086 6 : m1(i2c_, addr1), m2(i2c_, addr2), m3(i2c_, addr3), m4(i2c_, addr4)
fujikenac 0:d4e9f39c6086 7 {
fujikenac 0:d4e9f39c6086 8 oldangle = 0.0;
fujikenac 0:d4e9f39c6086 9 alpha = PI / 4 + phase * (PI / 2); //phaseは初期角度の指定 右に90°で-1, 左に90°で+1
fujikenac 0:d4e9f39c6086 10 }
fujikenac 0:d4e9f39c6086 11
fujikenac 0:d4e9f39c6086 12 /* うごけー */
fujikenac 0:d4e9f39c6086 13 void mecanum2017::move(int xdate, int ydate) //xdate, ydateは -64 ~ +64
fujikenac 0:d4e9f39c6086 14 {
fujikenac 0:d4e9f39c6086 15 if(xdate == 0 && ydate == 0) stop();
fujikenac 0:d4e9f39c6086 16 double angle = atan2(double(ydate), double(xdate)); //入力の角度
fujikenac 0:d4e9f39c6086 17 if(angle == oldangle); //前回と角度が同じだったらそのまま
fujikenac 0:d4e9f39c6086 18 else {
fujikenac 0:d4e9f39c6086 19 m1.control(float(sin(angle - alpha)));
fujikenac 0:d4e9f39c6086 20 m2.control(float(sin(angle + alpha)));
fujikenac 0:d4e9f39c6086 21 m3.control(float(sin(angle - alpha)));
fujikenac 0:d4e9f39c6086 22 m4.control(float(sin(angle + alpha)));
fujikenac 0:d4e9f39c6086 23 }
fujikenac 0:d4e9f39c6086 24 oldangle = angle; // 今回の角度を記憶
fujikenac 0:d4e9f39c6086 25 }
fujikenac 0:d4e9f39c6086 26
fujikenac 0:d4e9f39c6086 27 /* まわれー */
fujikenac 0:d4e9f39c6086 28 void mecanum2017::rotation(int rxdate)
fujikenac 0:d4e9f39c6086 29 {
fujikenac 0:d4e9f39c6086 30 m1.control(-rxdate * 0.015625); // 1 / 64 = 0.015625
fujikenac 0:d4e9f39c6086 31 m2.control( rxdate * 0.015625);
fujikenac 0:d4e9f39c6086 32 m3.control( rxdate * 0.015625);
fujikenac 0:d4e9f39c6086 33 m4.control(-rxdate * 0.015625);
fujikenac 0:d4e9f39c6086 34 }
fujikenac 0:d4e9f39c6086 35
fujikenac 0:d4e9f39c6086 36 /* とまれー */
fujikenac 0:d4e9f39c6086 37 void mecanum2017::stop()
fujikenac 0:d4e9f39c6086 38 {
fujikenac 0:d4e9f39c6086 39 m1.stop();
fujikenac 0:d4e9f39c6086 40 m2.stop();
fujikenac 0:d4e9f39c6086 41 m3.stop();
fujikenac 0:d4e9f39c6086 42 m4.stop();
fujikenac 0:d4e9f39c6086 43 }