おぼぼぼぼぼぼぼぼぼぼぼぼぼぼぼ
mecanum2017_2.cpp
- Committer:
- fujikenac
- Date:
- 2017-10-25
- Revision:
- 1:b92b7b117776
- Parent:
- 0:7d6d030c6a1b
File content as of revision 1:b92b7b117776:
#include "mbed.h" #include "T_motor.h" #include "P_motor.h" #include "mecanum2017_2.h" double mecanum2017_2::get_angle(short xdata, short ydata) { if(xdata || ydata) return atan2((double)ydata, (double)xdata); else return 0; } short mecanum2017_2::get_magnitude(short xdata, short ydata, short max) { short magnitude = (short)(sqrt((double)xdata * xdata + ydata * ydata)); if(magnitude > max) return max; else return magnitude; } short mecanum2017_2::map(short value, short fromLow, short fromHigh, short toLow, short toHigh) { if(value < fromLow) return toLow; else if(value > fromHigh) return toHigh; return (value - fromLow) * (toHigh - toLow) / (fromHigh - fromLow) + toLow; } mecanum2017_2::mecanum2017_2(I2C* i2c_, float* para_, char addr[], float rate_, int phase)//paraはモーターの回転方向を±1で表すよ いつもは全部+1だよ : m1(i2c_, addr[0]), m2(i2c_, addr[1]), m3(i2c_, addr[2]), m4(i2c_, addr[3]) { para = para_; alpha = PI / 4; beta = phase * (PI / 2); //phaseは初期角度の指定 左に90°で-1, 右に90°で+1 set_rate(rate_); //speed = 1; } //paraはモーターの回転方向を±1で表すよ いつもは全部+1だよ /* うごけー */ void mecanum2017_2::move(short xdata_L, short ydata_L, short xdata_R) { xdata_R = map(xdata_R, -64, 64, -100, 100); float magnitude = get_magnitude(xdata_L, ydata_L, 64); magnitude = map(magnitude, 8, 64, 0, 100) / 100.0f; double angle = get_angle(xdata_L, ydata_L); float move_component[2] = {magnitude * (float)sin(angle + beta - alpha), magnitude * (float)sin(angle + beta + alpha) }; float roll_component[2] = {-xdata_R * 0.5f / 100.0f, xdata_R * 0.5f / 100.0f}; if(xdata_R) { move_component[0] *= 0.5f; move_component[1] *= 0.5f; } //if(magnitude || xdata_R) acceleration(); //else speed_reset(); m1 = para[0] * (move_component[0] + roll_component[0]); m2 = para[1] * (move_component[1] + roll_component[1]); m3 = para[2] * (move_component[0] + roll_component[1]); m4 = para[3] * (move_component[1] + roll_component[0]); m1.run(); m2.run(); m3.run(); m4.run(); } void mecanum2017_2::anglemove(double angle) { float move_component[2] = {(float)sin(angle + beta - alpha), (float)sin(angle + beta + alpha) }; //acceleration(); m1 = para[0] * move_component[0]; m2 = para[1] * move_component[1]; m3 = para[2] * move_component[0]; m4 = para[3] * move_component[1]; m1.run(); m2.run(); m3.run(); m4.run(); } /* とまれー */ void mecanum2017_2::stop() { m1.stop(); m2.stop(); m3.stop(); m4.stop(); //speed_reset(); } void mecanum2017_2::set_rate(double rate_) { for(short i = 0; i < 4; i++) { if(para[i] > 0) para[i] = rate_; else if(para[i] < 0) para[i] = -rate_; } } // void mecanum2017_2::acceleration() // { // int time = timer.read_ms(); // if(time < 1000) // { // if(time == 0) // { // timer.start(); // time = 10; // } // //if(time >= 1000) timer.stop(); // speed = time / 1000.0f; // } // else // { // timer.stop(); // speed = 1.0; // } // } // void mecanum2017_2::speed_reset() // { // speed = 0; // timer.stop(); // timer.reset(); // }