ken fuji / mecanum2017_2

おぼぼぼぼぼぼぼぼぼぼぼぼぼぼぼ

mecanum2017_2.cpp@1:b92b7b117776, 2017-10-25 (annotated)

Committer:
fujikenac
Date:
Wed Oct 25 09:35:35 2017 +0000
Revision:
1:b92b7b117776
Parent:
0:7d6d030c6a1b 
add change_rate

Who changed what in which revision?

UserRevisionLine numberNew contents of line
fujikenac 0:7d6d030c6a1b 1 #include "mbed.h"
fujikenac 0:7d6d030c6a1b 2 #include "T_motor.h"
fujikenac 1:b92b7b117776 3 #include "P_motor.h"
fujikenac 0:7d6d030c6a1b 4 #include "mecanum2017_2.h"
fujikenac 0:7d6d030c6a1b 5
fujikenac 0:7d6d030c6a1b 6 double mecanum2017_2::get_angle(short xdata, short ydata)
fujikenac 0:7d6d030c6a1b 7 {
fujikenac 0:7d6d030c6a1b 8 if(xdata || ydata) return atan2((double)ydata, (double)xdata);
fujikenac 0:7d6d030c6a1b 9 else return 0;
fujikenac 0:7d6d030c6a1b 10 }
fujikenac 0:7d6d030c6a1b 11
fujikenac 0:7d6d030c6a1b 12 short mecanum2017_2::get_magnitude(short xdata, short ydata, short max)
fujikenac 0:7d6d030c6a1b 13 {
fujikenac 1:b92b7b117776 14 short magnitude = (short)(sqrt((double)xdata * xdata + ydata * ydata));
fujikenac 0:7d6d030c6a1b 15 if(magnitude > max) return max;
fujikenac 0:7d6d030c6a1b 16 else return magnitude;
fujikenac 0:7d6d030c6a1b 17 }
fujikenac 0:7d6d030c6a1b 18
fujikenac 0:7d6d030c6a1b 19 short mecanum2017_2::map(short value, short fromLow, short fromHigh, short toLow, short toHigh)
fujikenac 0:7d6d030c6a1b 20 {
fujikenac 1:b92b7b117776 21 if(value < fromLow) return toLow;
fujikenac 1:b92b7b117776 22 else if(value > fromHigh) return toHigh;
fujikenac 0:7d6d030c6a1b 23 return (value - fromLow) * (toHigh - toLow) / (fromHigh - fromLow) + toLow;
fujikenac 0:7d6d030c6a1b 24 }
fujikenac 0:7d6d030c6a1b 25
fujikenac 0:7d6d030c6a1b 26 mecanum2017_2::mecanum2017_2(I2C* i2c_, float* para_, char addr[], float rate_, int phase)//paraはモーターの回転方向を±1で表すよ いつもは全部+1だよ
fujikenac 0:7d6d030c6a1b 27 : m1(i2c_, addr[0]), m2(i2c_, addr[1]), m3(i2c_, addr[2]), m4(i2c_, addr[3])
fujikenac 0:7d6d030c6a1b 28 {
fujikenac 0:7d6d030c6a1b 29 para = para_;
fujikenac 0:7d6d030c6a1b 30 alpha = PI / 4;
fujikenac 0:7d6d030c6a1b 31 beta = phase * (PI / 2); //phaseは初期角度の指定 左に90°で-1, 右に90°で+1
fujikenac 1:b92b7b117776 32 set_rate(rate_);
fujikenac 1:b92b7b117776 33 //speed = 1;
fujikenac 0:7d6d030c6a1b 34 } //paraはモーターの回転方向を±1で表すよ いつもは全部+1だよ
fujikenac 0:7d6d030c6a1b 35
fujikenac 0:7d6d030c6a1b 36 /* うごけー */
fujikenac 0:7d6d030c6a1b 37 void mecanum2017_2::move(short xdata_L, short ydata_L, short xdata_R)
fujikenac 0:7d6d030c6a1b 38 {
fujikenac 0:7d6d030c6a1b 39 xdata_R = map(xdata_R, -64, 64, -100, 100);
fujikenac 1:b92b7b117776 40 float magnitude = get_magnitude(xdata_L, ydata_L, 64);
fujikenac 1:b92b7b117776 41 magnitude = map(magnitude, 8, 64, 0, 100) / 100.0f;
fujikenac 0:7d6d030c6a1b 42 double angle = get_angle(xdata_L, ydata_L);
fujikenac 0:7d6d030c6a1b 43 float move_component[2] = {magnitude * (float)sin(angle + beta - alpha),
fujikenac 0:7d6d030c6a1b 44 magnitude * (float)sin(angle + beta + alpha)
fujikenac 0:7d6d030c6a1b 45 };
fujikenac 1:b92b7b117776 46 float roll_component[2] = {-xdata_R * 0.5f / 100.0f, xdata_R * 0.5f / 100.0f};
fujikenac 1:b92b7b117776 47 if(xdata_R)
fujikenac 1:b92b7b117776 48 {
fujikenac 1:b92b7b117776 49 move_component[0] *= 0.5f;
fujikenac 1:b92b7b117776 50 move_component[1] *= 0.5f;
fujikenac 1:b92b7b117776 51 }
fujikenac 1:b92b7b117776 52 //if(magnitude || xdata_R) acceleration();
fujikenac 1:b92b7b117776 53 //else speed_reset();
fujikenac 0:7d6d030c6a1b 54
fujikenac 1:b92b7b117776 55 m1 = para[0] * (move_component[0] + roll_component[0]);
fujikenac 1:b92b7b117776 56 m2 = para[1] * (move_component[1] + roll_component[1]);
fujikenac 1:b92b7b117776 57 m3 = para[2] * (move_component[0] + roll_component[1]);
fujikenac 1:b92b7b117776 58 m4 = para[3] * (move_component[1] + roll_component[0]);
fujikenac 1:b92b7b117776 59 m1.run();
fujikenac 1:b92b7b117776 60 m2.run();
fujikenac 1:b92b7b117776 61 m3.run();
fujikenac 1:b92b7b117776 62 m4.run();
fujikenac 0:7d6d030c6a1b 63 }
fujikenac 0:7d6d030c6a1b 64
fujikenac 0:7d6d030c6a1b 65 void mecanum2017_2::anglemove(double angle)
fujikenac 0:7d6d030c6a1b 66 {
fujikenac 0:7d6d030c6a1b 67 float move_component[2] = {(float)sin(angle + beta - alpha),
fujikenac 0:7d6d030c6a1b 68 (float)sin(angle + beta + alpha)
fujikenac 0:7d6d030c6a1b 69 };
fujikenac 1:b92b7b117776 70 //acceleration();
fujikenac 1:b92b7b117776 71 m1 = para[0] * move_component[0];
fujikenac 1:b92b7b117776 72 m2 = para[1] * move_component[1];
fujikenac 1:b92b7b117776 73 m3 = para[2] * move_component[0];
fujikenac 1:b92b7b117776 74 m4 = para[3] * move_component[1];
fujikenac 1:b92b7b117776 75 m1.run();
fujikenac 1:b92b7b117776 76 m2.run();
fujikenac 1:b92b7b117776 77 m3.run();
fujikenac 1:b92b7b117776 78 m4.run();
fujikenac 0:7d6d030c6a1b 79 }
fujikenac 0:7d6d030c6a1b 80
fujikenac 0:7d6d030c6a1b 81 /* とまれー */
fujikenac 0:7d6d030c6a1b 82 void mecanum2017_2::stop()
fujikenac 0:7d6d030c6a1b 83 {
fujikenac 0:7d6d030c6a1b 84 m1.stop();
fujikenac 0:7d6d030c6a1b 85 m2.stop();
fujikenac 0:7d6d030c6a1b 86 m3.stop();
fujikenac 0:7d6d030c6a1b 87 m4.stop();
fujikenac 1:b92b7b117776 88 //speed_reset();
fujikenac 1:b92b7b117776 89 }
fujikenac 1:b92b7b117776 90
fujikenac 1:b92b7b117776 91 void mecanum2017_2::set_rate(double rate_)
fujikenac 1:b92b7b117776 92 {
fujikenac 1:b92b7b117776 93 for(short i = 0; i < 4; i++)
fujikenac 1:b92b7b117776 94 {
fujikenac 1:b92b7b117776 95 if(para[i] > 0)
fujikenac 1:b92b7b117776 96 para[i] = rate_;
fujikenac 1:b92b7b117776 97 else if(para[i] < 0)
fujikenac 1:b92b7b117776 98 para[i] = -rate_;
fujikenac 1:b92b7b117776 99 }
fujikenac 0:7d6d030c6a1b 100 }
fujikenac 0:7d6d030c6a1b 101
fujikenac 1:b92b7b117776 102 // void mecanum2017_2::acceleration()
fujikenac 1:b92b7b117776 103 // {
fujikenac 1:b92b7b117776 104 // int time = timer.read_ms();
fujikenac 1:b92b7b117776 105 // if(time < 1000)
fujikenac 1:b92b7b117776 106 // {
fujikenac 1:b92b7b117776 107 // if(time == 0)
fujikenac 1:b92b7b117776 108 // {
fujikenac 1:b92b7b117776 109 // timer.start();
fujikenac 1:b92b7b117776 110 // time = 10;
fujikenac 1:b92b7b117776 111 // }
fujikenac 1:b92b7b117776 112 // //if(time >= 1000) timer.stop();
fujikenac 1:b92b7b117776 113 // speed = time / 1000.0f;
fujikenac 1:b92b7b117776 114 // }
fujikenac 1:b92b7b117776 115 // else
fujikenac 1:b92b7b117776 116 // {
fujikenac 1:b92b7b117776 117 // timer.stop();
fujikenac 1:b92b7b117776 118 // speed = 1.0;
fujikenac 1:b92b7b117776 119 // }
fujikenac 1:b92b7b117776 120 // }
fujikenac 1:b92b7b117776 121
fujikenac 1:b92b7b117776 122 // void mecanum2017_2::speed_reset()
fujikenac 1:b92b7b117776 123 // {
fujikenac 1:b92b7b117776 124 // speed = 0;
fujikenac 1:b92b7b117776 125 // timer.stop();
fujikenac 1:b92b7b117776 126 // timer.reset();
fujikenac 1:b92b7b117776 127 // }