![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
3/2
Dependencies: mbed move4wheel2 EC CruizCore_R1370P
maxonsetting/maxonsetting.cpp@3:8a0faa3b08c3, 2019-03-01 (annotated)
- Committer:
- yuki0701
- Date:
- Fri Mar 01 00:48:07 2019 +0000
- Revision:
- 3:8a0faa3b08c3
- Parent:
- 0:c61c6e4775ca
a;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
la00noix | 0:c61c6e4775ca | 1 | #include "mbed.h" |
la00noix | 0:c61c6e4775ca | 2 | #include "math.h" |
la00noix | 0:c61c6e4775ca | 3 | #include "movement.h" |
la00noix | 0:c61c6e4775ca | 4 | #include "maxonsetting.h" |
la00noix | 0:c61c6e4775ca | 5 | |
la00noix | 0:c61c6e4775ca | 6 | Serial pc(USBTX,USBRX); |
la00noix | 0:c61c6e4775ca | 7 | void debug_printf(const char* format,...); // work as printf in debug |
la00noix | 0:c61c6e4775ca | 8 | void Debug_Control(); // control by PC keybord |
la00noix | 0:c61c6e4775ca | 9 | |
la00noix | 0:c61c6e4775ca | 10 | #define SPI_FREQ 1000000 // 1MHz |
la00noix | 0:c61c6e4775ca | 11 | #define SPI_BITS 16 |
la00noix | 0:c61c6e4775ca | 12 | #define SPI_MODE 0 |
la00noix | 0:c61c6e4775ca | 13 | #define SPI_WAIT_US 1 // 1us |
la00noix | 0:c61c6e4775ca | 14 | |
la00noix | 0:c61c6e4775ca | 15 | /*モーターの配置 |
la00noix | 0:c61c6e4775ca | 16 | * md1//---F---\\md4 |
la00noix | 0:c61c6e4775ca | 17 | * | | |
la00noix | 0:c61c6e4775ca | 18 | * L + R |
la00noix | 0:c61c6e4775ca | 19 | * | | |
la00noix | 0:c61c6e4775ca | 20 | * md2\\---B---//md3 |
la00noix | 0:c61c6e4775ca | 21 | */ |
la00noix | 0:c61c6e4775ca | 22 | |
la00noix | 0:c61c6e4775ca | 23 | //-----mbed----------------------------------------------------------------------------// |
la00noix | 0:c61c6e4775ca | 24 | SPI spi(p5,p6,p7); |
la00noix | 0:c61c6e4775ca | 25 | |
yuki0701 | 3:8a0faa3b08c3 | 26 | DigitalOut ss_md1(p17); //エスコンの設定 |
yuki0701 | 3:8a0faa3b08c3 | 27 | DigitalOut ss_md2(p18); |
yuki0701 | 3:8a0faa3b08c3 | 28 | DigitalOut ss_md3(p15); |
yuki0701 | 3:8a0faa3b08c3 | 29 | DigitalOut ss_md4(p16); |
la00noix | 0:c61c6e4775ca | 30 | |
la00noix | 0:c61c6e4775ca | 31 | DigitalOut md_enable(p25); |
la00noix | 0:c61c6e4775ca | 32 | //DigitalIn md_ch_enable(p10); // check enable switch is open or close |
la00noix | 0:c61c6e4775ca | 33 | //Timer md_disable; |
la00noix | 0:c61c6e4775ca | 34 | DigitalOut md_stop(p24); // stop all motor |
la00noix | 0:c61c6e4775ca | 35 | //DigitalIn md_check(p23); // check error of all motor driver //とりあえず使わない |
la00noix | 0:c61c6e4775ca | 36 | |
la00noix | 0:c61c6e4775ca | 37 | DigitalOut debug_led(LED3); //maxon debug programme -> on |
la00noix | 0:c61c6e4775ca | 38 | |
la00noix | 0:c61c6e4775ca | 39 | void UserLoopSetting_maxon() |
la00noix | 0:c61c6e4775ca | 40 | { |
la00noix | 0:c61c6e4775ca | 41 | //-----エスコンの初期設定-----// |
la00noix | 0:c61c6e4775ca | 42 | spi.format(SPI_BITS, SPI_MODE); |
la00noix | 0:c61c6e4775ca | 43 | spi.frequency(SPI_FREQ); |
la00noix | 0:c61c6e4775ca | 44 | ss_md1 = 1; |
la00noix | 0:c61c6e4775ca | 45 | ss_md2 = 1; |
la00noix | 0:c61c6e4775ca | 46 | ss_md3 = 1; |
la00noix | 0:c61c6e4775ca | 47 | ss_md4 = 1; |
la00noix | 0:c61c6e4775ca | 48 | md_enable = 1; //enable on |
la00noix | 0:c61c6e4775ca | 49 | md_stop = 1; |
la00noix | 0:c61c6e4775ca | 50 | |
la00noix | 0:c61c6e4775ca | 51 | #ifdef DEBUG_MAXON |
la00noix | 0:c61c6e4775ca | 52 | debug_led = 1; |
la00noix | 0:c61c6e4775ca | 53 | pc.attach(Debug_Control, Serial::RxIrq); |
la00noix | 0:c61c6e4775ca | 54 | #else |
la00noix | 0:c61c6e4775ca | 55 | debug_led = 0; |
la00noix | 0:c61c6e4775ca | 56 | #endif |
la00noix | 0:c61c6e4775ca | 57 | |
la00noix | 0:c61c6e4775ca | 58 | #ifdef DEBUG_PRINT |
la00noix | 0:c61c6e4775ca | 59 | debug_led = 1; |
la00noix | 0:c61c6e4775ca | 60 | #else |
la00noix | 0:c61c6e4775ca | 61 | debug_led = 0; |
la00noix | 0:c61c6e4775ca | 62 | #endif |
la00noix | 0:c61c6e4775ca | 63 | } |
la00noix | 0:c61c6e4775ca | 64 | |
la00noix | 0:c61c6e4775ca | 65 | #define MCP4922_AB (1<<15) |
la00noix | 0:c61c6e4775ca | 66 | #define MCP4922_BUF (1<<14) |
la00noix | 0:c61c6e4775ca | 67 | #define MCP4922_GA (1<<13) |
la00noix | 0:c61c6e4775ca | 68 | #define MCP4922_SHDN (1<<12) |
la00noix | 0:c61c6e4775ca | 69 | |
la00noix | 0:c61c6e4775ca | 70 | #define MCP4922_SET_OUTA (0x3000) //( MCP4922_GA || MCP4922_SHDN ) //12288 |
la00noix | 0:c61c6e4775ca | 71 | #define MCP4922_SET_OUTB (0xB000) //( MCP4922_AB || MCP4922_GA || MCP4922_SHDN ) //45056 |
la00noix | 0:c61c6e4775ca | 72 | #define MCP4922_MASKSET (0x0FFF) //4095 |
la00noix | 0:c61c6e4775ca | 73 | |
la00noix | 0:c61c6e4775ca | 74 | void DAC_Write(int16_t data, DigitalOut* DAC_cs)//(出力,出力場所) |
la00noix | 0:c61c6e4775ca | 75 | { |
la00noix | 0:c61c6e4775ca | 76 | static uint16_t dataA; //送るデータ |
la00noix | 0:c61c6e4775ca | 77 | static uint16_t dataB; |
la00noix | 0:c61c6e4775ca | 78 | |
la00noix | 0:c61c6e4775ca | 79 | dataA = MCP4922_SET_OUTA; |
la00noix | 0:c61c6e4775ca | 80 | dataB = MCP4922_SET_OUTB; |
la00noix | 0:c61c6e4775ca | 81 | |
la00noix | 0:c61c6e4775ca | 82 | if(data >= 0) { |
la00noix | 0:c61c6e4775ca | 83 | if(data > 4095) { |
la00noix | 0:c61c6e4775ca | 84 | data = 4095; |
la00noix | 0:c61c6e4775ca | 85 | } |
la00noix | 0:c61c6e4775ca | 86 | dataA += (MCP4922_MASKSET & (uint16_t)(data)); |
la00noix | 0:c61c6e4775ca | 87 | } else { |
la00noix | 0:c61c6e4775ca | 88 | if(data < -4095) { |
la00noix | 0:c61c6e4775ca | 89 | data = -4095; |
la00noix | 0:c61c6e4775ca | 90 | } |
la00noix | 0:c61c6e4775ca | 91 | dataB += (MCP4922_MASKSET & (uint16_t)(-data)); |
la00noix | 0:c61c6e4775ca | 92 | } |
la00noix | 0:c61c6e4775ca | 93 | |
la00noix | 0:c61c6e4775ca | 94 | //Aの出力設定 |
la00noix | 0:c61c6e4775ca | 95 | (DigitalOut)(*DAC_cs)=0; |
la00noix | 0:c61c6e4775ca | 96 | wait_us(SPI_WAIT_US); |
la00noix | 0:c61c6e4775ca | 97 | spi.write(dataA); |
la00noix | 0:c61c6e4775ca | 98 | wait_us(SPI_WAIT_US); |
la00noix | 0:c61c6e4775ca | 99 | (DigitalOut)(*DAC_cs)=1; |
la00noix | 0:c61c6e4775ca | 100 | wait_us(SPI_WAIT_US); |
la00noix | 0:c61c6e4775ca | 101 | |
la00noix | 0:c61c6e4775ca | 102 | //Bの出力設定 |
la00noix | 0:c61c6e4775ca | 103 | (DigitalOut)(*DAC_cs)=0; |
la00noix | 0:c61c6e4775ca | 104 | wait_us(SPI_WAIT_US); |
la00noix | 0:c61c6e4775ca | 105 | spi.write(dataB); |
la00noix | 0:c61c6e4775ca | 106 | wait_us(SPI_WAIT_US); |
la00noix | 0:c61c6e4775ca | 107 | (DigitalOut)(*DAC_cs)=1; |
la00noix | 0:c61c6e4775ca | 108 | |
la00noix | 0:c61c6e4775ca | 109 | } |
la00noix | 0:c61c6e4775ca | 110 | |
la00noix | 0:c61c6e4775ca | 111 | void MaxonControl(int16_t val_md1, int16_t val_md2, int16_t val_md3, int16_t val_md4) //出力 |
la00noix | 0:c61c6e4775ca | 112 | { |
la00noix | 0:c61c6e4775ca | 113 | static int16_t zero_check; |
la00noix | 0:c61c6e4775ca | 114 | |
la00noix | 0:c61c6e4775ca | 115 | DAC_Write(val_md1, &ss_md1); |
la00noix | 0:c61c6e4775ca | 116 | DAC_Write(val_md2, &ss_md2); |
la00noix | 0:c61c6e4775ca | 117 | DAC_Write(val_md3, &ss_md3); |
la00noix | 0:c61c6e4775ca | 118 | DAC_Write(val_md4, &ss_md4); |
la00noix | 0:c61c6e4775ca | 119 | |
la00noix | 0:c61c6e4775ca | 120 | zero_check = (val_md1 | val_md2 | val_md3 | val_md4); //すべての出力が0なら強制停止 |
la00noix | 0:c61c6e4775ca | 121 | if(zero_check == 0) { |
la00noix | 0:c61c6e4775ca | 122 | md_stop = 1; |
la00noix | 0:c61c6e4775ca | 123 | //md_stop_led = 1; |
la00noix | 0:c61c6e4775ca | 124 | } else { |
la00noix | 0:c61c6e4775ca | 125 | md_stop = 0; |
la00noix | 0:c61c6e4775ca | 126 | //md_stop_led = 0; |
la00noix | 0:c61c6e4775ca | 127 | } |
la00noix | 0:c61c6e4775ca | 128 | } |
la00noix | 0:c61c6e4775ca | 129 | |
la00noix | 0:c61c6e4775ca | 130 | void Debug_Control() |
la00noix | 0:c61c6e4775ca | 131 | { |
la00noix | 0:c61c6e4775ca | 132 | static char pc_command = '\0'; |
la00noix | 0:c61c6e4775ca | 133 | |
la00noix | 0:c61c6e4775ca | 134 | pc_command = pc.getc(); |
la00noix | 0:c61c6e4775ca | 135 | |
la00noix | 0:c61c6e4775ca | 136 | if(pc_command == 'w') { //前進 |
la00noix | 0:c61c6e4775ca | 137 | m1+=500; |
la00noix | 0:c61c6e4775ca | 138 | m2+=500; |
la00noix | 0:c61c6e4775ca | 139 | m3-=500; |
la00noix | 0:c61c6e4775ca | 140 | m4-=500; |
la00noix | 0:c61c6e4775ca | 141 | } else if(pc_command == 's') { //後進 |
la00noix | 0:c61c6e4775ca | 142 | m1-=500; |
la00noix | 0:c61c6e4775ca | 143 | m2-=500; |
la00noix | 0:c61c6e4775ca | 144 | m3+=500; |
la00noix | 0:c61c6e4775ca | 145 | m4+=500; |
la00noix | 0:c61c6e4775ca | 146 | } else if(pc_command == 'd') { //右回り |
la00noix | 0:c61c6e4775ca | 147 | m1+=500; |
la00noix | 0:c61c6e4775ca | 148 | m2+=500; |
la00noix | 0:c61c6e4775ca | 149 | m3+=500; |
la00noix | 0:c61c6e4775ca | 150 | m4+=500; |
la00noix | 0:c61c6e4775ca | 151 | } else if(pc_command == 'a') { //左回り |
la00noix | 0:c61c6e4775ca | 152 | m1-=500; |
la00noix | 0:c61c6e4775ca | 153 | m2-=500; |
la00noix | 0:c61c6e4775ca | 154 | m3-=500; |
la00noix | 0:c61c6e4775ca | 155 | m4-=500; |
la00noix | 0:c61c6e4775ca | 156 | } else { |
la00noix | 0:c61c6e4775ca | 157 | m1=0; |
la00noix | 0:c61c6e4775ca | 158 | m2=0; |
la00noix | 0:c61c6e4775ca | 159 | m3=0; |
la00noix | 0:c61c6e4775ca | 160 | m4=0; |
la00noix | 0:c61c6e4775ca | 161 | } |
la00noix | 0:c61c6e4775ca | 162 | |
la00noix | 0:c61c6e4775ca | 163 | if(m1>4095) { //最大値を超えないように |
la00noix | 0:c61c6e4775ca | 164 | m1=4095; |
la00noix | 0:c61c6e4775ca | 165 | } else if(m1<-4095) { |
la00noix | 0:c61c6e4775ca | 166 | m1=-4095; |
la00noix | 0:c61c6e4775ca | 167 | } |
la00noix | 0:c61c6e4775ca | 168 | if(m2>4095) { |
la00noix | 0:c61c6e4775ca | 169 | m2=4095; |
la00noix | 0:c61c6e4775ca | 170 | } else if(m2<-4095) { |
la00noix | 0:c61c6e4775ca | 171 | m2=-4095; |
la00noix | 0:c61c6e4775ca | 172 | } |
la00noix | 0:c61c6e4775ca | 173 | if(m3>4095) { |
la00noix | 0:c61c6e4775ca | 174 | m3=4095; |
la00noix | 0:c61c6e4775ca | 175 | } else if(m3<-4095) { |
la00noix | 0:c61c6e4775ca | 176 | m3=-4095; |
la00noix | 0:c61c6e4775ca | 177 | } |
la00noix | 0:c61c6e4775ca | 178 | if(m4>4095) { |
la00noix | 0:c61c6e4775ca | 179 | m4=4095; |
la00noix | 0:c61c6e4775ca | 180 | } else if(m4<-4095) { |
la00noix | 0:c61c6e4775ca | 181 | m4=-4095; |
la00noix | 0:c61c6e4775ca | 182 | } |
la00noix | 0:c61c6e4775ca | 183 | |
la00noix | 0:c61c6e4775ca | 184 | debug_printf("%d %d %d %d\r\n",m1,m2,m3,m4); |
la00noix | 0:c61c6e4775ca | 185 | MaxonControl(m1,m2,m3,m4); |
la00noix | 0:c61c6e4775ca | 186 | pc_command = '\0'; |
la00noix | 0:c61c6e4775ca | 187 | } |
la00noix | 0:c61c6e4775ca | 188 | |
la00noix | 0:c61c6e4775ca | 189 | void debug_printf(const char* format,...) |
la00noix | 0:c61c6e4775ca | 190 | { |
la00noix | 0:c61c6e4775ca | 191 | va_list arg; |
la00noix | 0:c61c6e4775ca | 192 | va_start(arg, format); |
la00noix | 0:c61c6e4775ca | 193 | vprintf(format, arg); |
la00noix | 0:c61c6e4775ca | 194 | va_end(arg); |
la00noix | 0:c61c6e4775ca | 195 | } |