ハヤト タジリ / MAIDROBO

スネーク

Motor.cpp@0:772b2085fe23, 2018-10-18 (annotated)

Committer:
tajiri1999
Date:
Thu Oct 18 09:57:12 2018 +0000
Revision:
0:772b2085fe23
????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tajiri1999 0:772b2085fe23 1 #include "mbed.h"
tajiri1999 0:772b2085fe23 2 #include "Motor.h"
tajiri1999 0:772b2085fe23 3
tajiri1999 0:772b2085fe23 4 Motor::Motor(PinName p01,PinName p10,PinName p02,PinName p20,PinName p03,PinName p30,PinName p04,PinName p40):
tajiri1999 0:772b2085fe23 5 _p01(p01),_p10(p10),_p02(p02),_p20(p20),_p03(p03),_p30(p30),_p04(p04),_p40(p40)
tajiri1999 0:772b2085fe23 6 {
tajiri1999 0:772b2085fe23 7 _p01 = 0;
tajiri1999 0:772b2085fe23 8 _p10 = 0;
tajiri1999 0:772b2085fe23 9 _p02 = 0;
tajiri1999 0:772b2085fe23 10 _p20 = 0;
tajiri1999 0:772b2085fe23 11 _p03 = 0;
tajiri1999 0:772b2085fe23 12 _p30 = 0;
tajiri1999 0:772b2085fe23 13 _p04 = 0;
tajiri1999 0:772b2085fe23 14 _p40 = 0;
tajiri1999 0:772b2085fe23 15 }
tajiri1999 0:772b2085fe23 16
tajiri1999 0:772b2085fe23 17 /*モーター周波数変更初期設定*/
tajiri1999 0:772b2085fe23 18 void Motor::setPwmPeriod(float hz){
tajiri1999 0:772b2085fe23 19 _p01.period(hz);
tajiri1999 0:772b2085fe23 20 _p10.period(hz);
tajiri1999 0:772b2085fe23 21 _p02.period(hz);
tajiri1999 0:772b2085fe23 22 _p20.period(hz);
tajiri1999 0:772b2085fe23 23 _p03.period(hz);
tajiri1999 0:772b2085fe23 24 _p30.period(hz);
tajiri1999 0:772b2085fe23 25 _p04.period(hz);
tajiri1999 0:772b2085fe23 26 _p40.period(hz);
tajiri1999 0:772b2085fe23 27 }
tajiri1999 0:772b2085fe23 28
tajiri1999 0:772b2085fe23 29 /*モーター制御*/
tajiri1999 0:772b2085fe23 30 void Motor::setPower(float a,float b,float c,float d){
tajiri1999 0:772b2085fe23 31
tajiri1999 0:772b2085fe23 32 a = a / 100;
tajiri1999 0:772b2085fe23 33 b = b / 100;
tajiri1999 0:772b2085fe23 34 c = c / 100;
tajiri1999 0:772b2085fe23 35 d = d / 100;
tajiri1999 0:772b2085fe23 36
tajiri1999 0:772b2085fe23 37 if(a > 0) { //正回転
tajiri1999 0:772b2085fe23 38 _p01 = a;
tajiri1999 0:772b2085fe23 39 _p10 = 0;
tajiri1999 0:772b2085fe23 40 } else if(a == 0) { //ブレーキ
tajiri1999 0:772b2085fe23 41 _p01 = 1;
tajiri1999 0:772b2085fe23 42 _p10 = 1;//0か1だと思う
tajiri1999 0:772b2085fe23 43 } else { //負回転
tajiri1999 0:772b2085fe23 44 _p01 = 0;
tajiri1999 0:772b2085fe23 45 _p10 = -1*a;
tajiri1999 0:772b2085fe23 46 }
tajiri1999 0:772b2085fe23 47 if(b > 0) {
tajiri1999 0:772b2085fe23 48 _p02 = b;
tajiri1999 0:772b2085fe23 49 _p20 = 0;
tajiri1999 0:772b2085fe23 50 } else if(b == 0) {
tajiri1999 0:772b2085fe23 51 _p02 = 1;
tajiri1999 0:772b2085fe23 52 _p20 = 1;
tajiri1999 0:772b2085fe23 53 } else {
tajiri1999 0:772b2085fe23 54 _p02 = 0;
tajiri1999 0:772b2085fe23 55 _p20 = -1*b;
tajiri1999 0:772b2085fe23 56 }
tajiri1999 0:772b2085fe23 57 if(c > 0) {
tajiri1999 0:772b2085fe23 58 _p03 = c;
tajiri1999 0:772b2085fe23 59 _p30 = 0;
tajiri1999 0:772b2085fe23 60 } else if(c == 0) {
tajiri1999 0:772b2085fe23 61 _p03 = 1;
tajiri1999 0:772b2085fe23 62 _p30 = 1;
tajiri1999 0:772b2085fe23 63 } else {
tajiri1999 0:772b2085fe23 64 _p03 = 0;
tajiri1999 0:772b2085fe23 65 _p30 = -1*c;
tajiri1999 0:772b2085fe23 66
tajiri1999 0:772b2085fe23 67 }
tajiri1999 0:772b2085fe23 68 if(d > 0) {
tajiri1999 0:772b2085fe23 69 _p04 = d;
tajiri1999 0:772b2085fe23 70 _p40 = 0;
tajiri1999 0:772b2085fe23 71 } else if(d == 0) {
tajiri1999 0:772b2085fe23 72 _p04 = 1;
tajiri1999 0:772b2085fe23 73 _p40 = 1;
tajiri1999 0:772b2085fe23 74 } else {
tajiri1999 0:772b2085fe23 75 _p04 = 0;
tajiri1999 0:772b2085fe23 76 _p40 = -1*d;
tajiri1999 0:772b2085fe23 77 }
tajiri1999 0:772b2085fe23 78 }
tajiri1999 0:772b2085fe23 79
tajiri1999 0:772b2085fe23 80 //*********************************************************************************/////////
tajiri1999 0:772b2085fe23 81 ///////モーター制御θ(degree)に進行方向,powerにPWMの値(0～100),修正値をmodに代入,右回転基準//////////
tajiri1999 0:772b2085fe23 82 //********************************************************************************://///////
tajiri1999 0:772b2085fe23 83 /*4輪用*/
tajiri1999 0:772b2085fe23 84 void Motor::omni4Wheels(int degree,int power,int mod){
tajiri1999 0:772b2085fe23 85 float motor[4];
tajiri1999 0:772b2085fe23 86 float Max[3];
tajiri1999 0:772b2085fe23 87 if(power == 0) { //パワー0のとき
tajiri1999 0:772b2085fe23 88
tajiri1999 0:772b2085fe23 89 motor[0] = -mod; //モーター右前
tajiri1999 0:772b2085fe23 90 motor[1] = -mod; //モーター右後
tajiri1999 0:772b2085fe23 91 motor[2] = -mod; //モーター左後
tajiri1999 0:772b2085fe23 92 motor[3] = -mod; //モーター左前
tajiri1999 0:772b2085fe23 93 } else {
tajiri1999 0:772b2085fe23 94
tajiri1999 0:772b2085fe23 95 motor[0] = sin((degree-45)*Pi/180) - mod; //モーター右前
tajiri1999 0:772b2085fe23 96 motor[1] = sin((degree-135)*Pi/180) - mod; //モーター右後
tajiri1999 0:772b2085fe23 97 motor[2] = sin((degree-225)*Pi/180) - mod; //モーター左後
tajiri1999 0:772b2085fe23 98 motor[3] = sin((degree-315)*Pi/180) - mod; //モーター左前
tajiri1999 0:772b2085fe23 99
tajiri1999 0:772b2085fe23 100 if(motor[0]>1) {
tajiri1999 0:772b2085fe23 101 motor[0] = 1;
tajiri1999 0:772b2085fe23 102 } else if(motor[0] < -1) {
tajiri1999 0:772b2085fe23 103 motor[0] = -1;
tajiri1999 0:772b2085fe23 104 }
tajiri1999 0:772b2085fe23 105 if(motor[1]>1) {
tajiri1999 0:772b2085fe23 106 motor[1] = 1;
tajiri1999 0:772b2085fe23 107 } else if(motor[1] < -1) {
tajiri1999 0:772b2085fe23 108 motor[1] = -1;
tajiri1999 0:772b2085fe23 109 }
tajiri1999 0:772b2085fe23 110 if(motor[2]>1) {
tajiri1999 0:772b2085fe23 111 motor[2] = 1;
tajiri1999 0:772b2085fe23 112 } else if(motor[2] < -1) {
tajiri1999 0:772b2085fe23 113 motor[2] = -1;
tajiri1999 0:772b2085fe23 114 }
tajiri1999 0:772b2085fe23 115 if(motor[3]>1) {
tajiri1999 0:772b2085fe23 116 motor[3] = 1;
tajiri1999 0:772b2085fe23 117 } else if(motor[3] < -1) {
tajiri1999 0:772b2085fe23 118 motor[3] = -1;
tajiri1999 0:772b2085fe23 119 }
tajiri1999 0:772b2085fe23 120
tajiri1999 0:772b2085fe23 121
tajiri1999 0:772b2085fe23 122 if(fabs(motor[0]) >= fabs(motor[1])) { //モーターパワー最大値計算
tajiri1999 0:772b2085fe23 123 Max[0] = fabs(motor[0]);
tajiri1999 0:772b2085fe23 124 } else {
tajiri1999 0:772b2085fe23 125 Max[0] = fabs(motor[1]);
tajiri1999 0:772b2085fe23 126 }
tajiri1999 0:772b2085fe23 127 if(fabs(motor[2]) >= fabs(motor[3])) {
tajiri1999 0:772b2085fe23 128 Max[1] = fabs(motor[2]);
tajiri1999 0:772b2085fe23 129 } else {
tajiri1999 0:772b2085fe23 130 Max[1] = fabs(motor[3]);
tajiri1999 0:772b2085fe23 131 }
tajiri1999 0:772b2085fe23 132 if(Max[0] >= Max[1]) {
tajiri1999 0:772b2085fe23 133 Max[2] = Max[0];
tajiri1999 0:772b2085fe23 134 } else {
tajiri1999 0:772b2085fe23 135 Max[2] = Max[1];
tajiri1999 0:772b2085fe23 136 }
tajiri1999 0:772b2085fe23 137
tajiri1999 0:772b2085fe23 138 motor[0] = (power*(motor[0]/Max[2]));//モータパワー最大値修正
tajiri1999 0:772b2085fe23 139 motor[1] = (power*(motor[1]/Max[2]));
tajiri1999 0:772b2085fe23 140 motor[2] = (power*(motor[2]/Max[2]));
tajiri1999 0:772b2085fe23 141 motor[3] = (power*(motor[3]/Max[2]));
tajiri1999 0:772b2085fe23 142 }
tajiri1999 0:772b2085fe23 143
tajiri1999 0:772b2085fe23 144 setPower(motor[0],motor[1],motor[2],motor[3]);
tajiri1999 0:772b2085fe23 145 }
tajiri1999 0:772b2085fe23 146
tajiri1999 0:772b2085fe23 147
tajiri1999 0:772b2085fe23 148 /*3輪用*/
tajiri1999 0:772b2085fe23 149 void Motor::omni3Wheels(int degree,int power,int mod){
tajiri1999 0:772b2085fe23 150 float motor[3];//モーター用変数
tajiri1999 0:772b2085fe23 151 float Max[2];//最大値用変数
tajiri1999 0:772b2085fe23 152
tajiri1999 0:772b2085fe23 153 if(power == 0){//パワー0のとき
tajiri1999 0:772b2085fe23 154
tajiri1999 0:772b2085fe23 155 motor[0] = -mod; //モーター右
tajiri1999 0:772b2085fe23 156 motor[1] = -mod; //モーター後
tajiri1999 0:772b2085fe23 157 motor[2] = -mod; //モーター左
tajiri1999 0:772b2085fe23 158 }
tajiri1999 0:772b2085fe23 159 else{
tajiri1999 0:772b2085fe23 160
tajiri1999 0:772b2085fe23 161 motor[0] = sin((degree-60)*Pi/180) - mod; //モーター右
tajiri1999 0:772b2085fe23 162 motor[1] = sin((degree-180)*Pi/180) - mod; //モーター後
tajiri1999 0:772b2085fe23 163 motor[2] = sin((degree-300)*Pi/180) - mod; //モーター左
tajiri1999 0:772b2085fe23 164
tajiri1999 0:772b2085fe23 165 if(motor[0]>1){
tajiri1999 0:772b2085fe23 166 motor[0] = 1;
tajiri1999 0:772b2085fe23 167 }
tajiri1999 0:772b2085fe23 168 else if(motor[0] < -1){
tajiri1999 0:772b2085fe23 169 motor[0] = -1;
tajiri1999 0:772b2085fe23 170 }
tajiri1999 0:772b2085fe23 171 if(motor[1]>1){
tajiri1999 0:772b2085fe23 172 motor[1] = 1;
tajiri1999 0:772b2085fe23 173 }
tajiri1999 0:772b2085fe23 174 else if(motor[1] < -1){
tajiri1999 0:772b2085fe23 175 motor[1] = -1;
tajiri1999 0:772b2085fe23 176 }
tajiri1999 0:772b2085fe23 177 if(motor[2]>1){
tajiri1999 0:772b2085fe23 178 motor[2] = 1;
tajiri1999 0:772b2085fe23 179 }
tajiri1999 0:772b2085fe23 180 else if(motor[2] < -1){
tajiri1999 0:772b2085fe23 181 motor[2] = -1;
tajiri1999 0:772b2085fe23 182 }
tajiri1999 0:772b2085fe23 183
tajiri1999 0:772b2085fe23 184
tajiri1999 0:772b2085fe23 185 if(fabs(motor[0]) >= fabs(motor[1])){ //モーターパワー最大値計算
tajiri1999 0:772b2085fe23 186 Max[0] = fabs(motor[0]);
tajiri1999 0:772b2085fe23 187 }
tajiri1999 0:772b2085fe23 188 else{
tajiri1999 0:772b2085fe23 189 Max[0] = fabs(motor[1]);
tajiri1999 0:772b2085fe23 190 }
tajiri1999 0:772b2085fe23 191 if(fabs(motor[2]) >= Max[0]){
tajiri1999 0:772b2085fe23 192 Max[1] = fabs(motor[2]);
tajiri1999 0:772b2085fe23 193 }
tajiri1999 0:772b2085fe23 194 else{
tajiri1999 0:772b2085fe23 195 Max[1] = Max[0];//最大値
tajiri1999 0:772b2085fe23 196 }
tajiri1999 0:772b2085fe23 197
tajiri1999 0:772b2085fe23 198
tajiri1999 0:772b2085fe23 199 motor[0] = (power*(motor[0]/Max[1]));//モータパワー最大値修正
tajiri1999 0:772b2085fe23 200 motor[1] = (power*(motor[1]/Max[1]));
tajiri1999 0:772b2085fe23 201 motor[2] = (power*(motor[2]/Max[1]));
tajiri1999 0:772b2085fe23 202 }
tajiri1999 0:772b2085fe23 203
tajiri1999 0:772b2085fe23 204 setPower(motor[0],motor[1],motor[2],0);
tajiri1999 0:772b2085fe23 205
tajiri1999 0:772b2085fe23 206 }
tajiri1999 0:772b2085fe23 207