harurobo_mbed_undercarriage_sub
Revision 0:9b75a5e505d0, committed 2018-12-22
- Comitter:
- yuki0701
- Date:
- Sat Dec 22 02:50:21 2018 +0000
- Commit message:
- a
Changed in this revision
Maxon_setting.cpp | Show annotated file Show diff for this revision Revisions of this file |
Maxon_setting.h | Show annotated file Show diff for this revision Revisions of this file |
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Maxon_setting.cpp Sat Dec 22 02:50:21 2018 +0000 @@ -0,0 +1,245 @@ +#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 \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Maxon_setting.h Sat Dec 22 02:50:21 2018 +0000 @@ -0,0 +1,29 @@ +#ifndef HARUROBO2019_MAXON_SETTING_H +#define HARUROBO2019_MAXON_SETTING_H +#include "mbed.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 + +static int16_t m1=0, m2=0, m3=0, m4=0; //int16bit = int2byte + +void UserLoopSetting(); + +void DAC_Write(int16_t data, DigitalOut* DAC_cs); + +void MotorControl(int16_t val_md1, int16_t val_md2, int16_t val_md3, int16_t val_md4); + + +#ifdef DEBUG_MODE +void Debug_Control(); //m1,m2,m3,m4の値がこのライブラリ内の変数となっているので恐らくこのままではこの関数は動作しない。改良の必要あり。 +#endif + + +#ifdef DEBUG_PRINT +void debug_printf(const char* format,...); +#endif + +#endif \ No newline at end of file