main.cpp

00001 #include "mbed.h"
00002 /*
00003 #include "IncEncoder.h" //インクリメント方式エンコーダ用ライブラリ
00004 #include "KRS.h" //近藤ICSサーボ用ライブラリ
00005 */
00006 #include <math.h>
00007 
00008 //円周率
00009 #define PI 3.14159265358979
00010 
00011 //モータードライバのアドレス
00012 #define DR1_MOTOR 0x01
00013 #define DR2_MOTOR 0x02
00014 #define DR3_MOTOR 0x03
00015 #define DR4_MOTOR 0x04
00016 
00017 //モータードライバへの命令
00018 #define STOP 0
00019 #define CW 1
00020 #define CCW -1
00021 #define bSTOP 10
00022 #define bCW 11
00023 #define bCCW -11
00024 
00025 #define sCW 21
00026 #define sCCW -21
00027 #define sSTOP 20
00028 #define shCW 31
00029 #define shCCW -31
00030 
00031 #define lCW 51
00032 #define lCCW -51
00033 #define lSTOP 50
00034 #define lsCW 61
00035 #define lsCCW -61
00036 #define lsSTOP 60
00037 
00038 #define rON 41
00039 #define rOFF -41
00040 
00041 Ticker timer; //767マザー動作不安定
00042 
00043 Serial controller_uart(PA_0,PA_1,19200);
00044 I2C md_i2c_tx(PD_13,PD_12);
00045 
00046 DigitalOut led1(PC_10);
00047 DigitalOut led2(PD_7);
00048 //DigitalOut led3(PD_6);
00049 //DigitalOut led4(PD_4);
00050 
00051 volatile unsigned char controller_data[10] = { 0x00, 0x00, 0x00, 0x00, 0x80, 0x80, 0x80, 0x80 };
00052 volatile unsigned char controller_data_number = 0;
00053 volatile unsigned char controller_cmd_getflag = 0;
00054 
00055 double lx, ly, lraw_rad, lclearance, ldistance, langle;
00056 double rx, ry, rraw_rad, rclearance, rdistance, rangle;
00057 
00058 //プロトタイプ関数宣言
00059 void read_controller_data();
00060 void con_check();
00061 void joy_cal();
00062 void md_setoutput(unsigned char address, int rotate, int duty);
00063 
00064 //main
00065 int main(){
00066     md_i2c_tx.frequency(400000); //MD用I2C
00067     timer.attach_us(&con_check, 100000);
00068     controller_uart.attach(&read_controller_data, Serial::RxIrq); //受信割り込み
00069     
00070     int rev1, rev2, rev3, rev4;
00071     
00072     double power1, power2, power3, power4;
00073     double power_up = 0;
00074     
00075     //ジョイスティックのオフセット
00076     lclearance = 60;
00077     rclearance = 90;
00078     
00079     led1 = 1;
00080     
00081     while(true){
00082         //モータ出力比計算
00083         power1 = sin((langle - 45)*PI/180);
00084         power2 = sin((langle - 135)*PI/180);
00085         power3 = sin((langle - 225)*PI/180);
00086         power4 = sin((langle - 315)*PI/180);
00087     
00088         if(power1 >= 0){
00089             rev1 = bCW;
00090         }else{
00091             rev1 = bCCW;
00092             power1 = -power1;
00093         }
00094         if(power2 >= 0){
00095             rev2 = bCW;
00096         }else{
00097             rev2 = bCCW;
00098             power2 = -power2;
00099         }
00100         if(power3 >= 0){
00101             rev3 = bCW;
00102         }else{
00103             rev3 = bCCW;
00104             power3 = -power3;
00105         }
00106         if(power4 >= 0){
00107             rev4 = bCW;
00108         }else{
00109             rev4 = bCCW;
00110             power4 = -power4;
00111         }
00112             
00113         //速度チェンジ
00114         if (controller_data[2] & 0x04){
00115             power_up = 1;
00116         }else{
00117             power_up = 0.8;
00118         }
00119         
00120         //足回り処理
00121         if(rx > 0){
00122             md_setoutput(DR1_MOTOR, bCW, rx * 0.5);
00123             md_setoutput(DR2_MOTOR, bCW, rx * 0.5);
00124             md_setoutput(DR3_MOTOR, bCW, rx * 0.5);
00125             md_setoutput(DR4_MOTOR, bCW, rx * 0.5);
00126         }else if(rx < 0){
00127             md_setoutput(DR1_MOTOR, bCCW, -rx * 0.5);
00128             md_setoutput(DR2_MOTOR, bCCW, -rx * 0.5);
00129             md_setoutput(DR3_MOTOR, bCCW, -rx * 0.5);
00130             md_setoutput(DR4_MOTOR, bCCW, -rx * 0.5);
00131         }else{
00132             if(ldistance > 50){
00133                 md_setoutput(DR1_MOTOR, rev1, (char)power1*ldistance*power_up);
00134                 md_setoutput(DR2_MOTOR, rev2, (char)power2*ldistance*power_up);
00135                 md_setoutput(DR3_MOTOR, rev3, (char)power3*ldistance*power_up);
00136                 md_setoutput(DR4_MOTOR, rev4, (char)power4*ldistance*power_up);
00137             }else{
00138                 md_setoutput(DR1_MOTOR, bSTOP, 0);
00139                 md_setoutput(DR2_MOTOR, bSTOP, 0);
00140                 md_setoutput(DR3_MOTOR, bSTOP, 0);
00141                 md_setoutput(DR4_MOTOR, bSTOP, 0);
00142             }
00143         }
00144         
00145         wait_ms(10);
00146     }
00147 }
00148 
00149 //コントローラー受信処理
00150 void read_controller_data() {
00151     unsigned char rxdata;
00152     
00153     rxdata = controller_uart.getc();
00154     
00155     if (controller_data_number == 0 && rxdata == 'S')
00156         controller_data_number++;
00157     else if (controller_data_number >= 1 && controller_data_number <= 7) {
00158         if (controller_data_number <= 3)
00159             controller_data[controller_data_number] = ~rxdata;
00160         else
00161             controller_data[controller_data_number] = rxdata;
00162         controller_data_number++;
00163     }
00164     else if (controller_data_number == 8 && rxdata == 'E') {
00165         controller_cmd_getflag = 0x01;
00166         controller_data_number = 0;
00167         led2 = 1;
00168         joy_cal();
00169     }
00170 }
00171 
00172 void con_check(){
00173     if(controller_cmd_getflag & 0x02) {
00174         controller_cmd_getflag = 0;
00175         
00176         controller_data[1] = 0x00;
00177         controller_data[2] = 0x00;
00178         controller_data[3] = 0x00;
00179         controller_data[4] = 0x80;
00180         controller_data[5] = 0x80;
00181         controller_data[6] = 0x80;
00182         controller_data[7] = 0x80;
00183         
00184         led2 = 0;
00185     }else
00186         controller_cmd_getflag |= 0x02;
00187 }
00188 
00189 //dualsenceジョイスティック計算
00190 void joy_cal() {
00191     rx = -(128 - controller_data[4]) * 2;
00192     ry = (128 - controller_data[5]) * 2;
00193     if (rx >= 255) rx = 255;
00194     if (rx <= -255) rx = -255;
00195     if (ry >= 255) ry = 255;
00196     if (ry <= -255) ry = -255;
00197     rraw_rad = atan2(ry, rx) / PI;
00198     rdistance = hypot(rx, ry) - rclearance;
00199     rdistance *= (255 / (255 - rclearance));
00200     if (rdistance < 0) rdistance = 0;
00201     if (rdistance >= 255) rdistance = 255;
00202     rangle = (rraw_rad < 0 ? 2 + rraw_rad : rraw_rad) * 180;
00203     rangle = rdistance == 0 ? 0 : rangle;
00204 
00205     if (rx > 0) {
00206         rx -= rclearance;
00207         if (rx < 0) rx = 0;
00208     }
00209     else if (rx < 0) {
00210         rx += rclearance;
00211         if (rx > 0) rx = 0;
00212     }
00213     rx *= (255 / (255 - rclearance));
00214 
00215     lx = -(128 - controller_data[6]) * 2;
00216     ly = (128 - controller_data[7]) * 2;
00217     if (lx >= 255) lx = 255;
00218     if (lx <= -255) lx = -255;
00219     if (ly >= 255) ly = 255;
00220     if (ly <= -255) ly = -255;
00221     lraw_rad = atan2(ly, lx) / PI;
00222     ldistance = hypot(lx, ly) - lclearance;
00223     ldistance *= (255 / (255 - lclearance));
00224     if (ldistance < 0) ldistance = 0;
00225     if (ldistance >= 255) ldistance = 255;
00226     langle = (lraw_rad < 0 ? 2 + lraw_rad : lraw_rad) * 180;
00227     langle = ldistance == 0 ? 0 : langle;
00228 }
00229 
00230 //MD用I2C通信処理
00231 void md_setoutput(unsigned char address, int rotate, int duty){
00232     char data[2];
00233  
00234     // モーター駆動制御モード
00235     switch (rotate) {
00236         case CW:    data[0] = 'F'; break;// 電圧比例制御モード（PWMがLの区間はフリー）
00237         case CCW:   data[0] = 'R'; break;
00238         case STOP:  data[0] = 'S'; break;
00239     
00240         case bCW:   data[0] = 'f'; break; // 電圧比例制御モード（PWMがLの区間はブレーキ）
00241         case bCCW:  data[0] = 'r'; break;
00242         case bSTOP: data[0] = 's'; break;
00243     
00244         case sCW:   data[0] = 'A'; break; // 速度比例制御モード
00245         case sCCW:  data[0] = 'B'; break;
00246         case sSTOP: data[0] = 'T'; break;
00247     
00248         case shCW:   data[0] = 'a'; break; // 速度比例制御モード（命令パケット最上位ビットに指令値9ビット目を入れることで分解能2倍）
00249         case shCCW:  data[0] = 'b'; break;
00250     
00251         case lCW:   data[0] = 'L'; break; // 通常LAP
00252         case lCCW:  data[0] = 'M'; break;
00253         case lSTOP: data[0] = 'N'; break;
00254         case lsCW:   data[0] = 'l'; break; // 速調LAP
00255         case lsCCW:  data[0] = 'm'; break;
00256         case lsSTOP: data[0] = 'n'; break;
00257     
00258         case rON: data[0] = 'P'; break; //リレー駆動モード
00259         case rOFF: data[0] = 'p'; break;
00260     }
00261     
00262     data[1] = duty & 0xFF;
00263     md_i2c_tx.write(address << 1, data, 2); //duty送信
00264 }