オムニホイールの制御
ArduinoUNOの3輪オムニホイール用コード書きました！僕も不完全なところが多いですが,よければ自分で読み解いてください。自分で読み解けた人は、４輪でも５輪でも制御できる用になるはずです。```#define PI 3.141592653589793/*プロトタイプ宣言*/void motor(float deg,float power,float mod);/************************************************//*初期設定*//***********************************************/void setup(){ //右前モーター pinMode(3,OUTPUT); pinMode(5,OUTPUT); //後ろモーター pinMode(6,OUTPUT); pinMode(9,OUTPUT); //左前モーター pinMode(10,OUTPUT); pinMode(11,OUTPUT);}/***********************************************//*本文*//***********************************************/void loop(){ //モーターを動かす //(進みたい方向,パワー(0to100,0のときブレーキ),向きの修正(-1to1)) motor(90,100,0);//右に進む}/*********************************************//*関数名:motor(flaot deg,float power,float mod)*//*引数:(進みたい角度,パワー(0to100)0のときブレーキ,向き修正(回転)(-1to1)*//*戻り値：無し*//*概要:モーターを動かして,任意の方向に移動する*//*********************************************/void motor(float deg,float power,float mod){ float Motor[3]; float Max[2]; if(power == 0){//パワーが0のとき Motor[0] = 255 * mod; Motor[1] = 255 * mod; Motor[2] = 255 * mod; } else{ Motor[0] = sin((deg-60)*PI/180) + mod; Motor[1] = sin((deg-180)*PI/180) + mod; Motor[2] = sin((deg-300)*PI/180) + mod; if(Motor[0] > 1){ Motor[0] = 1; } else if(Motor[0] < -1){ Motor[0] = -1; } if(Motor[1] > 1){ Motor[1] = 1; } else if(Motor[1] < -1){ Motor[1] = -1; } if(Motor[2] > 1){ Motor[2] = 1; } else if(Motor[2] < -1){ Motor[2] = -1; } if(fabs(Motor[0]) >= fabs(Motor[1])){//モータパワー最大値計算 Max[0] = fabs(Motor[0]); } else{ Max[0] = fabs(Motor[1]); } if(fabs(Motor[2]) >= Max[0]){ Max[1] = fabs(Motor[2]); } else{ Max[1] = Max[0];//最大値 } //最大値変換 Motor[0] = (255*power*(Motor[0]/Max[1]))/100; Motor[1] = (255*power*(Motor[1]/Max[1]))/100; Motor[2] = (255*power*(Motor[2]/Max[1]))/100; } //右前モーター if(Motor[0] > 0){//正回転 analogWrite(3,(int)Motor[0]);//キャスト変換してPWM値を代入 analogWrite(5,0); } else if(Motor[0] == 0){//ブレーキ analogWrite(3,255); analogWrite(5,255); } else{//負回転 analogWrite(3,0); analogWrite(5,(int) -1*Motor[0]);//キャスト変換してPWM値を代入 } //後ろモーター if(Motor[1] > 0){//正回転 analogWrite(6,(int)Motor[1]);//キャスト変換してPWM値を代入 analogWrite(9,0); } else if(Motor[1] == 0){//ブレーキ analogWrite(6,255); analogWrite(9,255); } else{//負回転 analogWrite(6,0); analogWrite(9,(int) -1*Motor[1]);//キャスト変換してPWM値を代入 } //左前モーター if(Motor[2] > 0){//正回転 analogWrite(10,(int)Motor[2]);//キャスト変換してPWM値を代入 analogWrite(11,0); } else if(Motor[2] == 0){//ブレーキ analogWrite(10,255); analogWrite(11,255); } else{//負回転 analogWrite(10,0); analogWrite(11,(int) -1*Motor[2]);//キャスト変換してPWM値を代入 }}```
