Maxon_setting.cpp

00001 #include <Maxon_setting.h>
00002 //#include <mbed.h>
00003 //#include <math.h>
00004 //#include <stdarg.h>
00005 
00006 
00007 /*#define DEBUG_MODE                              // compile as debug mode (comment out if you don't use)
00008 #ifdef DEBUG_MODE
00009 #define DEBUG_PRINT                             // enable debug_printf
00010 #endif*/
00011 
00012 Serial pc(USBTX,USBRX);
00013 void debug_printf(const char* format,...);      // work as printf in debug
00014 void Debug_Control();                           // control by PC keybord
00015 
00016 #define SPI_FREQ    1000000         // 1MHz
00017 #define SPI_BITS    16
00018 #define SPI_MODE    0
00019 #define SPI_WAIT_US 1               // 1us
00020 
00021 /*モーターの配置
00022 *     md1//---F---\\md4
00023 *        |         |
00024 *        L    +    R
00025 *        |         |
00026 *     md2\\---B---//md3
00027 */
00028 
00029 //-----mbed-----//
00030 SPI spi(p5,p6,p7);
00031 //CAN can1(p30,p29,1000000);
00032 
00033 DigitalOut ss_md1(p15);           //エスコンの設定
00034 DigitalOut ss_md2(p16);
00035 DigitalOut ss_md3(p17);
00036 DigitalOut ss_md4(p18);
00037 
00038 DigitalOut md_enable(p25);
00039 //DigitalIn md_ch_enable(p10);        // check enable switch is open or close
00040 //Timer md_disable;
00041 DigitalOut md_stop(p24);          // stop all motor
00042 DigitalIn  md_check(p23);           // check error of all motor driver  //とりあえず使わない
00043 
00044 /*Ec EC1(p8,p26,NC,500,0.05);
00045 Ec EC2(p21,p22,NC,500,0.05);
00046 R1370P gyro(p28,p27);*/
00047 
00048 //Ticker motor_tick;  //角速度計算用ticker
00049 //Ticker ticker;  //for enc
00050 
00051 /*-----nucleo-----//
00052 SPI spi(PB_5,PB_4,PB_3);
00053 
00054 DigitalOut ss_md1(PB_15);           //エスコンの設定
00055 DigitalOut ss_md2(PB_14);
00056 DigitalOut ss_md3(PB_13);
00057 DigitalOut ss_md4(PC_4);
00058 
00059 DigitalOut md_enable(PA_13);        // do all motor driver enable
00060 //DigitalIn md_ch_enable(p10);        // check enable switch is open or close
00061 //Timer md_disable;
00062 DigitalOut md_stop(PA_14);          // stop all motor
00063 DigitalIn  md_check(PB_7);           // check error of all motor driver  //とりあえず使わない
00064 
00065 Ec EC1(PC_6,PC_8,NC,500,0.05);
00066 Ec EC2(PB_1,PB_12,NC,500,0.05);
00067 R1370P gyro(PC_6,PC_7);
00068 
00069 Ticker motor_tick; //角速度計算用ticker
00070 Ticker ticker;  //for enc  */
00071 
00072 
00073 
00074 //DigitalOut can_led(LED1);           //if can enable -> toggle
00075 DigitalOut debug_led(LED2);         //if debugmode -> on
00076 DigitalOut md_stop_led(LED3);       //if motor stop -> on
00077 DigitalOut md_err_led(LED4);        //if driver error -> on  //とりあえず使わない
00078 
00079 void UserLoopSetting()
00080 {
00081 //------機体情報の初期化------//
00082 
00083 //info.nowX = {0,0,0};
00084 //info.nowY = {0,0,0};
00085 
00086 //-----エスコンの初期設定-----//
00087     spi.format(SPI_BITS, SPI_MODE);
00088     spi.frequency(SPI_FREQ);
00089     ss_md1 = 1;
00090     ss_md2 = 1;
00091     ss_md3 = 1;
00092     ss_md4 = 1;
00093     md_enable = 1;  //enable on
00094     md_err_led = 0;
00095     md_stop = 1;
00096     md_stop_led = 1;
00097 //-----センサーの初期設定-----//
00098    /* gyro.initialize();
00099     motor_tick.attach(&calOmega,0.05);  //0.05秒間隔で角速度を計算
00100     EC1.setDiameter_mm(48);
00101     EC2.setDiameter_mm(48);  //測定輪半径//後で測定*/
00102 
00103 #ifdef DEBUG_MODE
00104     debug_led = 1;
00105     pc.attach(Debug_Control, Serial::RxIrq);
00106 #else
00107     debug_led = 0;
00108 #endif
00109 }
00110 
00111 #define MCP4922_AB      (1<<15)
00112 #define MCP4922_BUF     (1<<14)
00113 #define MCP4922_GA      (1<<13)
00114 #define MCP4922_SHDN    (1<<12)
00115 
00116 #define MCP4922_SET_OUTA    (0x3000)    //( MCP4922_GA || MCP4922_SHDN )  //12288
00117 #define MCP4922_SET_OUTB    (0xB000)    //( MCP4922_AB || MCP4922_GA || MCP4922_SHDN )  //45056
00118 #define MCP4922_MASKSET     (0x0FFF)    //4095
00119 
00120 void DAC_Write(int16_t data, DigitalOut* DAC_cs)//(出力,出力場所)
00121 {
00122     static uint16_t dataA;  //送るデータ
00123     static uint16_t dataB;
00124 
00125     dataA = MCP4922_SET_OUTA;
00126     dataB = MCP4922_SET_OUTB;
00127 
00128     if(data >= 0) {
00129         if(data > 4095) {
00130             data = 4095;
00131         }
00132         dataA += (MCP4922_MASKSET & (uint16_t)(data));
00133     } else {
00134         if(data < -4095) {
00135             data = -4095;
00136         }
00137         dataB += (MCP4922_MASKSET & (uint16_t)(-data));
00138     }
00139 
00140     //Aの出力設定
00141     (DigitalOut)(*DAC_cs)=0;
00142     wait_us(SPI_WAIT_US);
00143     spi.write(dataA);
00144     wait_us(SPI_WAIT_US);
00145     (DigitalOut)(*DAC_cs)=1;
00146     wait_us(SPI_WAIT_US);
00147 
00148     //Bの出力設定
00149     (DigitalOut)(*DAC_cs)=0;
00150     wait_us(SPI_WAIT_US);
00151     spi.write(dataB);
00152     wait_us(SPI_WAIT_US);
00153     (DigitalOut)(*DAC_cs)=1;
00154 
00155 }
00156 
00157 void MotorControl(int16_t val_md1, int16_t val_md2, int16_t val_md3, int16_t val_md4)   //出力
00158 {
00159     static int16_t zero_check;
00160 
00161     DAC_Write(val_md1, &ss_md1);
00162     DAC_Write(val_md2, &ss_md2);
00163     DAC_Write(val_md3, &ss_md3);
00164     DAC_Write(val_md4, &ss_md4);
00165 
00166     zero_check = (val_md1 | val_md2 | val_md3 | val_md4);  //すべての出力が0なら強制停止
00167     if(zero_check == 0) {
00168         md_stop = 1;
00169         md_stop_led = 1;
00170     } else {
00171         md_stop = 0;
00172         md_stop_led = 0;
00173     }
00174 }
00175 
00176 //#ifdef DEBUG_MODE
00177 void Debug_Control()
00178 {
00179     static char pc_command = '\0';
00180 
00181     pc_command = pc.getc();
00182 
00183     if(pc_command == 'w') {  //前進
00184         m1+=500;
00185         m2+=500;
00186         m3-=500;
00187         m4-=500;
00188     } else if(pc_command == 's') {  //後進
00189         m1-=500;
00190         m2-=500;
00191         m3+=500;
00192         m4+=500;
00193     } else if(pc_command == 'd') {  //右回り
00194         m1+=500;
00195         m2+=500;
00196         m3+=500;
00197         m4+=500;
00198     } else if(pc_command == 'a') {  //左回り
00199         m1-=500;
00200         m2-=500;
00201         m3-=500;
00202         m4-=500;
00203     } else {
00204         m1=0;
00205         m2=0;
00206         m3=0;
00207         m4=0;
00208     }
00209 
00210     if(m1>4095) {  //最大値を超えないように
00211         m1=4095;
00212     } else if(m1<-4095) {
00213         m1=-4095;
00214     }
00215     if(m2>4095) {
00216         m2=4095;
00217     } else if(m2<-4095) {
00218         m2=-4095;
00219     }
00220     if(m3>4095) {
00221         m3=4095;
00222     } else if(m3<-4095) {
00223         m3=-4095;
00224     }
00225     if(m4>4095) {
00226         m4=4095;
00227     } else if(m4<-4095) {
00228         m4=-4095;
00229     }
00230 
00231     debug_printf("%d %d %d %d\r\n",m1,m2,m3,m4);
00232     MotorControl(m1,m2,m3,m4);
00233     pc_command = '\0';
00234 }
00235 //#endif
00236 
00237 //#ifdef DEBUG_PRINT
00238 void debug_printf(const char* format,...)
00239 {
00240     va_list arg;
00241     va_start(arg, format);
00242     vprintf(format, arg);
00243     va_end(arg);
00244 }
00245 //#endif