harurobo_mbed_undercarriage_sub
Maxon_setting.cpp
- Committer:
- yuki0701
- Date:
- 2018-12-22
- Revision:
- 0:9b75a5e505d0
File content as of revision 0:9b75a5e505d0:
#include <Maxon_setting.h> //#include <mbed.h> //#include <math.h> //#include <stdarg.h> /*#define DEBUG_MODE // compile as debug mode (comment out if you don't use) #ifdef DEBUG_MODE #define DEBUG_PRINT // enable debug_printf #endif*/ 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); //CAN can1(p30,p29,1000000); 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 //とりあえず使わない /*Ec EC1(p8,p26,NC,500,0.05); Ec EC2(p21,p22,NC,500,0.05); R1370P gyro(p28,p27);*/ //Ticker motor_tick; //角速度計算用ticker //Ticker ticker; //for enc /*-----nucleo-----// SPI spi(PB_5,PB_4,PB_3); DigitalOut ss_md1(PB_15); //エスコンの設定 DigitalOut ss_md2(PB_14); DigitalOut ss_md3(PB_13); DigitalOut ss_md4(PC_4); DigitalOut md_enable(PA_13); // do all motor driver enable //DigitalIn md_ch_enable(p10); // check enable switch is open or close //Timer md_disable; DigitalOut md_stop(PA_14); // stop all motor DigitalIn md_check(PB_7); // check error of all motor driver //とりあえず使わない Ec EC1(PC_6,PC_8,NC,500,0.05); Ec EC2(PB_1,PB_12,NC,500,0.05); R1370P gyro(PC_6,PC_7); Ticker motor_tick; //角速度計算用ticker Ticker ticker; //for enc */ //DigitalOut can_led(LED1); //if can enable -> toggle DigitalOut debug_led(LED2); //if debugmode -> on DigitalOut md_stop_led(LED3); //if motor stop -> on DigitalOut md_err_led(LED4); //if driver error -> on //とりあえず使わない void UserLoopSetting() { //------機体情報の初期化------// //info.nowX = {0,0,0}; //info.nowY = {0,0,0}; //-----エスコンの初期設定-----// 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_err_led = 0; md_stop = 1; md_stop_led = 1; //-----センサーの初期設定-----// /* gyro.initialize(); motor_tick.attach(&calOmega,0.05); //0.05秒間隔で角速度を計算 EC1.setDiameter_mm(48); EC2.setDiameter_mm(48); //測定輪半径//後で測定*/ #ifdef DEBUG_MODE debug_led = 1; pc.attach(Debug_Control, Serial::RxIrq); #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 MotorControl(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; } } //#ifdef DEBUG_MODE 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); MotorControl(m1,m2,m3,m4); pc_command = '\0'; } //#endif //#ifdef DEBUG_PRINT void debug_printf(const char* format,...) { va_list arg; va_start(arg, format); vprintf(format, arg); va_end(arg); } //#endif