![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
3/2
Dependencies: mbed move4wheel2 EC CruizCore_R1370P
Diff: maxonsetting/maxonsetting.cpp
- Revision:
- 0:c61c6e4775ca
- Child:
- 3:8a0faa3b08c3
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/maxonsetting/maxonsetting.cpp Wed Feb 13 03:02:19 2019 +0000 @@ -0,0 +1,195 @@ +#include "mbed.h" +#include "math.h" +#include "movement.h" +#include "maxonsetting.h" + +Serial pc(USBTX,USBRX); +void debug_printf(const char* format,...); // work as printf in debug +void Debug_Control(); // control by PC keybord + +#define SPI_FREQ 1000000 // 1MHz +#define SPI_BITS 16 +#define SPI_MODE 0 +#define SPI_WAIT_US 1 // 1us + +/*モーターの配置 +* md1//---F---\\md4 +* | | +* L + R +* | | +* md2\\---B---//md3 +*/ + +//-----mbed----------------------------------------------------------------------------// +SPI spi(p5,p6,p7); + +DigitalOut ss_md1(p15); //エスコンの設定 +DigitalOut ss_md2(p16); +DigitalOut ss_md3(p17); +DigitalOut ss_md4(p18); + +DigitalOut md_enable(p25); +//DigitalIn md_ch_enable(p10); // check enable switch is open or close +//Timer md_disable; +DigitalOut md_stop(p24); // stop all motor +//DigitalIn md_check(p23); // check error of all motor driver //とりあえず使わない + +DigitalOut debug_led(LED3); //maxon debug programme -> on + +void UserLoopSetting_maxon() +{ +//-----エスコンの初期設定-----// + spi.format(SPI_BITS, SPI_MODE); + spi.frequency(SPI_FREQ); + ss_md1 = 1; + ss_md2 = 1; + ss_md3 = 1; + ss_md4 = 1; + md_enable = 1; //enable on + md_stop = 1; + +#ifdef DEBUG_MAXON + debug_led = 1; + pc.attach(Debug_Control, Serial::RxIrq); +#else + debug_led = 0; +#endif + +#ifdef DEBUG_PRINT + debug_led = 1; +#else + debug_led = 0; +#endif +} + +#define MCP4922_AB (1<<15) +#define MCP4922_BUF (1<<14) +#define MCP4922_GA (1<<13) +#define MCP4922_SHDN (1<<12) + +#define MCP4922_SET_OUTA (0x3000) //( MCP4922_GA || MCP4922_SHDN ) //12288 +#define MCP4922_SET_OUTB (0xB000) //( MCP4922_AB || MCP4922_GA || MCP4922_SHDN ) //45056 +#define MCP4922_MASKSET (0x0FFF) //4095 + +void DAC_Write(int16_t data, DigitalOut* DAC_cs)//(出力,出力場所) +{ + static uint16_t dataA; //送るデータ + static uint16_t dataB; + + dataA = MCP4922_SET_OUTA; + dataB = MCP4922_SET_OUTB; + + if(data >= 0) { + if(data > 4095) { + data = 4095; + } + dataA += (MCP4922_MASKSET & (uint16_t)(data)); + } else { + if(data < -4095) { + data = -4095; + } + dataB += (MCP4922_MASKSET & (uint16_t)(-data)); + } + + //Aの出力設定 + (DigitalOut)(*DAC_cs)=0; + wait_us(SPI_WAIT_US); + spi.write(dataA); + wait_us(SPI_WAIT_US); + (DigitalOut)(*DAC_cs)=1; + wait_us(SPI_WAIT_US); + + //Bの出力設定 + (DigitalOut)(*DAC_cs)=0; + wait_us(SPI_WAIT_US); + spi.write(dataB); + wait_us(SPI_WAIT_US); + (DigitalOut)(*DAC_cs)=1; + +} + +void MaxonControl(int16_t val_md1, int16_t val_md2, int16_t val_md3, int16_t val_md4) //出力 +{ + static int16_t zero_check; + + DAC_Write(val_md1, &ss_md1); + DAC_Write(val_md2, &ss_md2); + DAC_Write(val_md3, &ss_md3); + DAC_Write(val_md4, &ss_md4); + + zero_check = (val_md1 | val_md2 | val_md3 | val_md4); //すべての出力が0なら強制停止 + if(zero_check == 0) { + md_stop = 1; + //md_stop_led = 1; + } else { + md_stop = 0; + //md_stop_led = 0; + } +} + +void Debug_Control() +{ + static char pc_command = '\0'; + + pc_command = pc.getc(); + + if(pc_command == 'w') { //前進 + m1+=500; + m2+=500; + m3-=500; + m4-=500; + } else if(pc_command == 's') { //後進 + m1-=500; + m2-=500; + m3+=500; + m4+=500; + } else if(pc_command == 'd') { //右回り + m1+=500; + m2+=500; + m3+=500; + m4+=500; + } else if(pc_command == 'a') { //左回り + m1-=500; + m2-=500; + m3-=500; + m4-=500; + } else { + m1=0; + m2=0; + m3=0; + m4=0; + } + + if(m1>4095) { //最大値を超えないように + m1=4095; + } else if(m1<-4095) { + m1=-4095; + } + if(m2>4095) { + m2=4095; + } else if(m2<-4095) { + m2=-4095; + } + if(m3>4095) { + m3=4095; + } else if(m3<-4095) { + m3=-4095; + } + if(m4>4095) { + m4=4095; + } else if(m4<-4095) { + m4=-4095; + } + + debug_printf("%d %d %d %d\r\n",m1,m2,m3,m4); + MaxonControl(m1,m2,m3,m4); + pc_command = '\0'; +} + +void debug_printf(const char* format,...) +{ + va_list arg; + va_start(arg, format); + vprintf(format, arg); + va_end(arg); +} \ No newline at end of file