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@3:063d7878f0d1, 2017-08-18 (annotated)

Committer:
fujikenac
Date:
Fri Aug 18 07:16:50 2017 +0000
Revision:
3:063d7878f0d1
Parent:
2:678b5128a4d6
Child:
4:d4c2e7addc77
add para, anglemove

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 3:063d7878f0d1 5 mecanum2017::mecanum2017(I2C& i2c_, short para1_, char addr1, short para2_, char addr2,
fujikenac 3:063d7878f0d1 6 short para3_, char addr3, short para4_, char addr4, int phase) //paraはモーターの回転方向を±1で表すよ いつもは全部+1だよ
fujikenac 0:d4e9f39c6086 7 : m1(i2c_, addr1), m2(i2c_, addr2), m3(i2c_, addr3), m4(i2c_, addr4)
fujikenac 0:d4e9f39c6086 8 {
fujikenac 3:063d7878f0d1 9 para1 = para1_;
fujikenac 3:063d7878f0d1 10 para2 = para2_;
fujikenac 3:063d7878f0d1 11 para3 = para3_;
fujikenac 3:063d7878f0d1 12 para4 = para4_;
fujikenac 0:d4e9f39c6086 13 oldangle = 0.0;
fujikenac 3:063d7878f0d1 14 alpha = PI / 4;
fujikenac 3:063d7878f0d1 15 beta = phase * (PI / 2); //phaseは初期角度の指定 左に90°で-1, 右に90°で+1
fujikenac 0:d4e9f39c6086 16 }
fujikenac 0:d4e9f39c6086 17
fujikenac 0:d4e9f39c6086 18 /* うごけー */
fujikenac 0:d4e9f39c6086 19 void mecanum2017::move(int xdate, int ydate) //xdate, ydateは -64 ~ +64
fujikenac 0:d4e9f39c6086 20 {
fujikenac 2:678b5128a4d6 21 if(xdate == 0 && ydate == 0) free();
fujikenac 3:063d7878f0d1 22 double angle = atan2(double(ydate), double(xdate)) + beta; //入力の角度
fujikenac 0:d4e9f39c6086 23 if(angle == oldangle); //前回と角度が同じだったらそのまま
fujikenac 3:063d7878f0d1 24 else
fujikenac 3:063d7878f0d1 25 {
fujikenac 3:063d7878f0d1 26 m1.control(float(para1 * sin(angle - alpha)));
fujikenac 3:063d7878f0d1 27 m2.control(float(para2 * sin(angle + alpha)));
fujikenac 3:063d7878f0d1 28 m3.control(float(para3 * sin(angle - alpha)));
fujikenac 3:063d7878f0d1 29 m4.control(float(para4 * sin(angle + alpha)));
fujikenac 3:063d7878f0d1 30 }
fujikenac 3:063d7878f0d1 31 oldangle = angle; // 今回の角度を記憶
fujikenac 3:063d7878f0d1 32 }
fujikenac 3:063d7878f0d1 33
fujikenac 3:063d7878f0d1 34 /*かくどー*/
fujikenac 3:063d7878f0d1 35 void mecanum2017::anglemove(double angle)
fujikenac 3:063d7878f0d1 36 {
fujikenac 3:063d7878f0d1 37 angle += beta;
fujikenac 3:063d7878f0d1 38 if(angle == oldangle); //前回と角度が同じだったらそのまま
fujikenac 3:063d7878f0d1 39 else
fujikenac 3:063d7878f0d1 40 {
fujikenac 3:063d7878f0d1 41 m1.control(float(para1 * sin(angle - alpha)));
fujikenac 3:063d7878f0d1 42 m2.control(float(para2 * sin(angle + alpha)));
fujikenac 3:063d7878f0d1 43 m3.control(float(para3 * sin(angle - alpha)));
fujikenac 3:063d7878f0d1 44 m4.control(float(para4 * sin(angle + alpha)));
fujikenac 0:d4e9f39c6086 45 }
fujikenac 0:d4e9f39c6086 46 oldangle = angle; // 今回の角度を記憶
fujikenac 0:d4e9f39c6086 47 }
fujikenac 0:d4e9f39c6086 48
fujikenac 0:d4e9f39c6086 49 /* まわれー */
fujikenac 0:d4e9f39c6086 50 void mecanum2017::rotation(int rxdate)
fujikenac 0:d4e9f39c6086 51 {
fujikenac 3:063d7878f0d1 52 m1.control(para1 * -rxdate * 0.015625); // 1 / 64 = 0.015625
fujikenac 3:063d7878f0d1 53 m2.control(para2 * rxdate * 0.015625);
fujikenac 3:063d7878f0d1 54 m3.control(para3 * rxdate * 0.015625);
fujikenac 3:063d7878f0d1 55 m4.control(para4 * -rxdate * 0.015625);
fujikenac 0:d4e9f39c6086 56 }
fujikenac 0:d4e9f39c6086 57
fujikenac 0:d4e9f39c6086 58 /* とまれー */
fujikenac 0:d4e9f39c6086 59 void mecanum2017::stop()
fujikenac 0:d4e9f39c6086 60 {
fujikenac 0:d4e9f39c6086 61 m1.stop();
fujikenac 0:d4e9f39c6086 62 m2.stop();
fujikenac 0:d4e9f39c6086 63 m3.stop();
fujikenac 0:d4e9f39c6086 64 m4.stop();
fujikenac 1:cc63373e84ec 65 }
fujikenac 1:cc63373e84ec 66
fujikenac 1:cc63373e84ec 67 /*そのままー*/
fujikenac 1:cc63373e84ec 68 void mecanum2017::free()
fujikenac 1:cc63373e84ec 69 {
fujikenac 1:cc63373e84ec 70 m1.free();
fujikenac 1:cc63373e84ec 71 m2.free();
fujikenac 1:cc63373e84ec 72 m3.free();
fujikenac 1:cc63373e84ec 73 m4.free();
fujikenac 1:cc63373e84ec 74 }