maxonsetting.cpp

00001 #include "mbed.h"
00002 #include "math.h"
00003 #include "movement.h"
00004 #include "maxonsetting.h"
00005 
00006 Serial pc(USBTX,USBRX);
00007 void debug_printf(const char* format,...);      // work as printf in debug
00008 void Debug_Control();                           // control by PC keybord
00009 
00010 #define SPI_FREQ    1000000         // 1MHz
00011 #define SPI_BITS    16
00012 #define SPI_MODE    0
00013 #define SPI_WAIT_US 1               // 1us
00014 
00015 /*モーターの配置
00016 *     md1//---F---\\md4
00017 *        |         |
00018 *        L    +    R
00019 *        |         |
00020 *     md2\\---B---//md3
00021 */
00022 
00023 //-----mbed----------------------------------------------------------------------------//
00024 SPI spi(p5,p6,p7);
00025 
00026 DigitalOut ss_md1(p17);           //エスコンの設定
00027 DigitalOut ss_md2(p18);
00028 DigitalOut ss_md3(p15);
00029 DigitalOut ss_md4(p16);
00030 
00031 DigitalOut md_enable(p25);
00032 //DigitalIn md_ch_enable(p10);        // check enable switch is open or close
00033 //Timer md_disable;
00034 DigitalOut md_stop(p24);          // stop all motor
00035 //DigitalIn  md_check(p23);           // check error of all motor driver  //とりあえず使わない
00036 
00037 DigitalOut debug_led(LED3);  //maxon debug programme -> on
00038 
00039 void UserLoopSetting_maxon()
00040 {
00041 //-----エスコンの初期設定-----//
00042     spi.format(SPI_BITS, SPI_MODE);
00043     spi.frequency(SPI_FREQ);
00044     ss_md1 = 1;
00045     ss_md2 = 1;
00046     ss_md3 = 1;
00047     ss_md4 = 1;
00048     md_enable = 1;  //enable on
00049     md_stop = 1;
00050 
00051 #ifdef DEBUG_MAXON
00052     debug_led = 1;
00053     pc.attach(Debug_Control, Serial::RxIrq);
00054 #else
00055     debug_led = 0;
00056 #endif
00057 
00058 #ifdef DEBUG_PRINT
00059     debug_led = 1;
00060 #else
00061     debug_led = 0;
00062 #endif
00063 }
00064 
00065 #define MCP4922_AB      (1<<15)
00066 #define MCP4922_BUF     (1<<14)
00067 #define MCP4922_GA      (1<<13)
00068 #define MCP4922_SHDN    (1<<12)
00069 
00070 #define MCP4922_SET_OUTA    (0x3000)    //( MCP4922_GA || MCP4922_SHDN )  //12288
00071 #define MCP4922_SET_OUTB    (0xB000)    //( MCP4922_AB || MCP4922_GA || MCP4922_SHDN )  //45056
00072 #define MCP4922_MASKSET     (0x0FFF)    //4095
00073 
00074 void DAC_Write(int16_t data, DigitalOut* DAC_cs)//(出力,出力場所)
00075 {
00076     static uint16_t dataA;  //送るデータ
00077     static uint16_t dataB;
00078 
00079     dataA = MCP4922_SET_OUTA;
00080     dataB = MCP4922_SET_OUTB;
00081 
00082     if(data >= 0) {
00083         if(data > 4095) {
00084             data = 4095;
00085         }
00086         dataA += (MCP4922_MASKSET & (uint16_t)(data));
00087     } else {
00088         if(data < -4095) {
00089             data = -4095;
00090         }
00091         dataB += (MCP4922_MASKSET & (uint16_t)(-data));
00092     }
00093 
00094     //Aの出力設定
00095     (DigitalOut)(*DAC_cs)=0;
00096     wait_us(SPI_WAIT_US);
00097     spi.write(dataA);
00098     wait_us(SPI_WAIT_US);
00099     (DigitalOut)(*DAC_cs)=1;
00100     wait_us(SPI_WAIT_US);
00101 
00102     //Bの出力設定
00103     (DigitalOut)(*DAC_cs)=0;
00104     wait_us(SPI_WAIT_US);
00105     spi.write(dataB);
00106     wait_us(SPI_WAIT_US);
00107     (DigitalOut)(*DAC_cs)=1;
00108 
00109 }
00110 
00111 void MaxonControl(int16_t val_md1, int16_t val_md2, int16_t val_md3, int16_t val_md4)   //出力
00112 {
00113     static int16_t zero_check;
00114 
00115     DAC_Write(val_md1, &ss_md1);
00116     DAC_Write(val_md2, &ss_md2);
00117     DAC_Write(val_md3, &ss_md3);
00118     DAC_Write(val_md4, &ss_md4);
00119 
00120     zero_check = (val_md1 | val_md2 | val_md3 | val_md4);  //すべての出力が0なら強制停止
00121     if(zero_check == 0) {
00122         md_stop = 1;
00123         //md_stop_led = 1;
00124     } else {
00125         md_stop = 0;
00126         //md_stop_led = 0;
00127     }
00128 }
00129 
00130 void Debug_Control()
00131 {
00132     static char pc_command = '\0';
00133 
00134     pc_command = pc.getc();
00135 
00136     if(pc_command == 'w') {  //前進
00137         m1+=500;
00138         m2+=500;
00139         m3-=500;
00140         m4-=500;
00141     } else if(pc_command == 's') {  //後進
00142         m1-=500;
00143         m2-=500;
00144         m3+=500;
00145         m4+=500;
00146     } else if(pc_command == 'd') {  //右回り
00147         m1+=500;
00148         m2+=500;
00149         m3+=500;
00150         m4+=500;
00151     } else if(pc_command == 'a') {  //左回り
00152         m1-=500;
00153         m2-=500;
00154         m3-=500;
00155         m4-=500;
00156     } else {
00157         m1=0;
00158         m2=0;
00159         m3=0;
00160         m4=0;
00161     }
00162 
00163     if(m1>4095) {  //最大値を超えないように
00164         m1=4095;
00165     } else if(m1<-4095) {
00166         m1=-4095;
00167     }
00168     if(m2>4095) {
00169         m2=4095;
00170     } else if(m2<-4095) {
00171         m2=-4095;
00172     }
00173     if(m3>4095) {
00174         m3=4095;
00175     } else if(m3<-4095) {
00176         m3=-4095;
00177     }
00178     if(m4>4095) {
00179         m4=4095;
00180     } else if(m4<-4095) {
00181         m4=-4095;
00182     }
00183 
00184     debug_printf("%d %d %d %d\r\n",m1,m2,m3,m4);
00185     MaxonControl(m1,m2,m3,m4);
00186     pc_command = '\0';
00187 }
00188 
00189 void debug_printf(const char* format,...)
00190 {
00191     va_list arg;
00192     va_start(arg, format);
00193     vprintf(format, arg);
00194     va_end(arg);
00195 }