春ロボ1班(元F3RC4班+) / Maxon_setting_1_11

harurobo_mbed_undercarriage_sub

Maxon_setting.cpp@2:e20be362f2ac, 2019-01-11 (annotated)

Committer:
yuki0701
Date:
Fri Jan 11 08:28:19 2019 +0000
Revision:
2:e20be362f2ac
Parent:
1:4081cf9e86f4 
a;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yuki0701 0:9b75a5e505d0 1 #include <Maxon_setting.h>
yuki0701 1:4081cf9e86f4 2 #include <mbed.h>
yuki0701 1:4081cf9e86f4 3 #include <math.h>
yuki0701 1:4081cf9e86f4 4 #include <stdarg.h>
yuki0701 0:9b75a5e505d0 5
yuki0701 0:9b75a5e505d0 6
yuki0701 0:9b75a5e505d0 7 /*#define DEBUG_MODE // compile as debug mode (comment out if you don't use)
yuki0701 0:9b75a5e505d0 8 #ifdef DEBUG_MODE
yuki0701 0:9b75a5e505d0 9 #define DEBUG_PRINT // enable debug_printf
yuki0701 0:9b75a5e505d0 10 #endif*/
yuki0701 0:9b75a5e505d0 11
yuki0701 0:9b75a5e505d0 12 Serial pc(USBTX,USBRX);
yuki0701 0:9b75a5e505d0 13 void debug_printf(const char* format,...); // work as printf in debug
yuki0701 0:9b75a5e505d0 14 void Debug_Control(); // control by PC keybord
yuki0701 0:9b75a5e505d0 15
yuki0701 0:9b75a5e505d0 16 #define SPI_FREQ 1000000 // 1MHz
yuki0701 0:9b75a5e505d0 17 #define SPI_BITS 16
yuki0701 0:9b75a5e505d0 18 #define SPI_MODE 0
yuki0701 0:9b75a5e505d0 19 #define SPI_WAIT_US 1 // 1us
yuki0701 0:9b75a5e505d0 20
yuki0701 0:9b75a5e505d0 21 /*モーターの配置
yuki0701 0:9b75a5e505d0 22 * md1//---F---\\md4
yuki0701 0:9b75a5e505d0 23 * | |
yuki0701 0:9b75a5e505d0 24 * L + R
yuki0701 0:9b75a5e505d0 25 * | |
yuki0701 0:9b75a5e505d0 26 * md2\\---B---//md3
yuki0701 0:9b75a5e505d0 27 */
yuki0701 0:9b75a5e505d0 28
yuki0701 0:9b75a5e505d0 29 //-----mbed-----//
yuki0701 0:9b75a5e505d0 30 SPI spi(p5,p6,p7);
yuki0701 0:9b75a5e505d0 31 //CAN can1(p30,p29,1000000);
yuki0701 0:9b75a5e505d0 32
yuki0701 0:9b75a5e505d0 33 DigitalOut ss_md1(p15); //エスコンの設定
yuki0701 0:9b75a5e505d0 34 DigitalOut ss_md2(p16);
yuki0701 0:9b75a5e505d0 35 DigitalOut ss_md3(p17);
yuki0701 0:9b75a5e505d0 36 DigitalOut ss_md4(p18);
yuki0701 0:9b75a5e505d0 37
yuki0701 0:9b75a5e505d0 38 DigitalOut md_enable(p25);
yuki0701 0:9b75a5e505d0 39 //DigitalIn md_ch_enable(p10); // check enable switch is open or close
yuki0701 0:9b75a5e505d0 40 //Timer md_disable;
yuki0701 0:9b75a5e505d0 41 DigitalOut md_stop(p24); // stop all motor
yuki0701 0:9b75a5e505d0 42 DigitalIn md_check(p23); // check error of all motor driver //とりあえず使わない
yuki0701 0:9b75a5e505d0 43
yuki0701 0:9b75a5e505d0 44 /*Ec EC1(p8,p26,NC,500,0.05);
yuki0701 0:9b75a5e505d0 45 Ec EC2(p21,p22,NC,500,0.05);
yuki0701 0:9b75a5e505d0 46 R1370P gyro(p28,p27);*/
yuki0701 0:9b75a5e505d0 47
yuki0701 0:9b75a5e505d0 48 //Ticker motor_tick; //角速度計算用ticker
yuki0701 0:9b75a5e505d0 49 //Ticker ticker; //for enc
yuki0701 0:9b75a5e505d0 50
yuki0701 0:9b75a5e505d0 51 /*-----nucleo-----//
yuki0701 0:9b75a5e505d0 52 SPI spi(PB_5,PB_4,PB_3);
yuki0701 0:9b75a5e505d0 53
yuki0701 0:9b75a5e505d0 54 DigitalOut ss_md1(PB_15); //エスコンの設定
yuki0701 0:9b75a5e505d0 55 DigitalOut ss_md2(PB_14);
yuki0701 0:9b75a5e505d0 56 DigitalOut ss_md3(PB_13);
yuki0701 0:9b75a5e505d0 57 DigitalOut ss_md4(PC_4);
yuki0701 0:9b75a5e505d0 58
yuki0701 0:9b75a5e505d0 59 DigitalOut md_enable(PA_13); // do all motor driver enable
yuki0701 0:9b75a5e505d0 60 //DigitalIn md_ch_enable(p10); // check enable switch is open or close
yuki0701 0:9b75a5e505d0 61 //Timer md_disable;
yuki0701 0:9b75a5e505d0 62 DigitalOut md_stop(PA_14); // stop all motor
yuki0701 0:9b75a5e505d0 63 DigitalIn md_check(PB_7); // check error of all motor driver //とりあえず使わない
yuki0701 0:9b75a5e505d0 64
yuki0701 0:9b75a5e505d0 65 Ec EC1(PC_6,PC_8,NC,500,0.05);
yuki0701 0:9b75a5e505d0 66 Ec EC2(PB_1,PB_12,NC,500,0.05);
yuki0701 0:9b75a5e505d0 67 R1370P gyro(PC_6,PC_7);
yuki0701 0:9b75a5e505d0 68
yuki0701 0:9b75a5e505d0 69 Ticker motor_tick; //角速度計算用ticker
yuki0701 0:9b75a5e505d0 70 Ticker ticker; //for enc */
yuki0701 0:9b75a5e505d0 71
yuki0701 0:9b75a5e505d0 72
yuki0701 0:9b75a5e505d0 73
yuki0701 0:9b75a5e505d0 74 //DigitalOut can_led(LED1); //if can enable -> toggle
yuki0701 0:9b75a5e505d0 75 DigitalOut debug_led(LED2); //if debugmode -> on
yuki0701 0:9b75a5e505d0 76 DigitalOut md_stop_led(LED3); //if motor stop -> on
yuki0701 0:9b75a5e505d0 77 DigitalOut md_err_led(LED4); //if driver error -> on //とりあえず使わない
yuki0701 0:9b75a5e505d0 78
yuki0701 0:9b75a5e505d0 79 void UserLoopSetting()
yuki0701 0:9b75a5e505d0 80 {
yuki0701 0:9b75a5e505d0 81
yuki0701 0:9b75a5e505d0 82 //-----エスコンの初期設定-----//
yuki0701 0:9b75a5e505d0 83 spi.format(SPI_BITS, SPI_MODE);
yuki0701 0:9b75a5e505d0 84 spi.frequency(SPI_FREQ);
yuki0701 0:9b75a5e505d0 85 ss_md1 = 1;
yuki0701 0:9b75a5e505d0 86 ss_md2 = 1;
yuki0701 0:9b75a5e505d0 87 ss_md3 = 1;
yuki0701 0:9b75a5e505d0 88 ss_md4 = 1;
yuki0701 0:9b75a5e505d0 89 md_enable = 1; //enable on
yuki0701 0:9b75a5e505d0 90 md_err_led = 0;
yuki0701 0:9b75a5e505d0 91 md_stop = 1;
yuki0701 0:9b75a5e505d0 92 md_stop_led = 1;
yuki0701 0:9b75a5e505d0 93 //-----センサーの初期設定-----//
yuki0701 0:9b75a5e505d0 94 /* gyro.initialize();
yuki0701 0:9b75a5e505d0 95 motor_tick.attach(&calOmega,0.05); //0.05秒間隔で角速度を計算
yuki0701 0:9b75a5e505d0 96 EC1.setDiameter_mm(48);
yuki0701 0:9b75a5e505d0 97 EC2.setDiameter_mm(48); //測定輪半径//後で測定*/
yuki0701 0:9b75a5e505d0 98
yuki0701 0:9b75a5e505d0 99 #ifdef DEBUG_MODE
yuki0701 0:9b75a5e505d0 100 debug_led = 1;
yuki0701 0:9b75a5e505d0 101 pc.attach(Debug_Control, Serial::RxIrq);
yuki0701 0:9b75a5e505d0 102 #else
yuki0701 0:9b75a5e505d0 103 debug_led = 0;
yuki0701 0:9b75a5e505d0 104 #endif
yuki0701 0:9b75a5e505d0 105 }
yuki0701 0:9b75a5e505d0 106
yuki0701 0:9b75a5e505d0 107 #define MCP4922_AB (1<<15)
yuki0701 0:9b75a5e505d0 108 #define MCP4922_BUF (1<<14)
yuki0701 0:9b75a5e505d0 109 #define MCP4922_GA (1<<13)
yuki0701 0:9b75a5e505d0 110 #define MCP4922_SHDN (1<<12)
yuki0701 0:9b75a5e505d0 111
yuki0701 0:9b75a5e505d0 112 #define MCP4922_SET_OUTA (0x3000) //( MCP4922_GA || MCP4922_SHDN ) //12288
yuki0701 0:9b75a5e505d0 113 #define MCP4922_SET_OUTB (0xB000) //( MCP4922_AB || MCP4922_GA || MCP4922_SHDN ) //45056
yuki0701 0:9b75a5e505d0 114 #define MCP4922_MASKSET (0x0FFF) //4095
yuki0701 0:9b75a5e505d0 115
yuki0701 0:9b75a5e505d0 116 void DAC_Write(int16_t data, DigitalOut* DAC_cs)//(出力,出力場所)
yuki0701 0:9b75a5e505d0 117 {
yuki0701 0:9b75a5e505d0 118 static uint16_t dataA; //送るデータ
yuki0701 0:9b75a5e505d0 119 static uint16_t dataB;
yuki0701 0:9b75a5e505d0 120
yuki0701 0:9b75a5e505d0 121 dataA = MCP4922_SET_OUTA;
yuki0701 0:9b75a5e505d0 122 dataB = MCP4922_SET_OUTB;
yuki0701 0:9b75a5e505d0 123
yuki0701 0:9b75a5e505d0 124 if(data >= 0) {
yuki0701 0:9b75a5e505d0 125 if(data > 4095) {
yuki0701 0:9b75a5e505d0 126 data = 4095;
yuki0701 0:9b75a5e505d0 127 }
yuki0701 0:9b75a5e505d0 128 dataA += (MCP4922_MASKSET & (uint16_t)(data));
yuki0701 0:9b75a5e505d0 129 } else {
yuki0701 0:9b75a5e505d0 130 if(data < -4095) {
yuki0701 0:9b75a5e505d0 131 data = -4095;
yuki0701 0:9b75a5e505d0 132 }
yuki0701 0:9b75a5e505d0 133 dataB += (MCP4922_MASKSET & (uint16_t)(-data));
yuki0701 0:9b75a5e505d0 134 }
yuki0701 0:9b75a5e505d0 135
yuki0701 0:9b75a5e505d0 136 //Aの出力設定
yuki0701 0:9b75a5e505d0 137 (DigitalOut)(*DAC_cs)=0;
yuki0701 0:9b75a5e505d0 138 wait_us(SPI_WAIT_US);
yuki0701 0:9b75a5e505d0 139 spi.write(dataA);
yuki0701 0:9b75a5e505d0 140 wait_us(SPI_WAIT_US);
yuki0701 0:9b75a5e505d0 141 (DigitalOut)(*DAC_cs)=1;
yuki0701 0:9b75a5e505d0 142 wait_us(SPI_WAIT_US);
yuki0701 0:9b75a5e505d0 143
yuki0701 0:9b75a5e505d0 144 //Bの出力設定
yuki0701 0:9b75a5e505d0 145 (DigitalOut)(*DAC_cs)=0;
yuki0701 0:9b75a5e505d0 146 wait_us(SPI_WAIT_US);
yuki0701 0:9b75a5e505d0 147 spi.write(dataB);
yuki0701 0:9b75a5e505d0 148 wait_us(SPI_WAIT_US);
yuki0701 0:9b75a5e505d0 149 (DigitalOut)(*DAC_cs)=1;
yuki0701 0:9b75a5e505d0 150
yuki0701 0:9b75a5e505d0 151 }
yuki0701 0:9b75a5e505d0 152
yuki0701 0:9b75a5e505d0 153 void MotorControl(int16_t val_md1, int16_t val_md2, int16_t val_md3, int16_t val_md4) //出力
yuki0701 0:9b75a5e505d0 154 {
yuki0701 0:9b75a5e505d0 155 static int16_t zero_check;
yuki0701 0:9b75a5e505d0 156
yuki0701 0:9b75a5e505d0 157 DAC_Write(val_md1, &ss_md1);
yuki0701 0:9b75a5e505d0 158 DAC_Write(val_md2, &ss_md2);
yuki0701 0:9b75a5e505d0 159 DAC_Write(val_md3, &ss_md3);
yuki0701 0:9b75a5e505d0 160 DAC_Write(val_md4, &ss_md4);
yuki0701 0:9b75a5e505d0 161
yuki0701 0:9b75a5e505d0 162 zero_check = (val_md1 | val_md2 | val_md3 | val_md4); //すべての出力が0なら強制停止
yuki0701 0:9b75a5e505d0 163 if(zero_check == 0) {
yuki0701 0:9b75a5e505d0 164 md_stop = 1;
yuki0701 0:9b75a5e505d0 165 md_stop_led = 1;
yuki0701 0:9b75a5e505d0 166 } else {
yuki0701 0:9b75a5e505d0 167 md_stop = 0;
yuki0701 0:9b75a5e505d0 168 md_stop_led = 0;
yuki0701 0:9b75a5e505d0 169 }
yuki0701 0:9b75a5e505d0 170 }
yuki0701 0:9b75a5e505d0 171
yuki0701 0:9b75a5e505d0 172 //#ifdef DEBUG_MODE
yuki0701 0:9b75a5e505d0 173 void Debug_Control()
yuki0701 0:9b75a5e505d0 174 {
yuki0701 0:9b75a5e505d0 175 static char pc_command = '\0';
yuki0701 0:9b75a5e505d0 176
yuki0701 0:9b75a5e505d0 177 pc_command = pc.getc();
yuki0701 0:9b75a5e505d0 178
yuki0701 0:9b75a5e505d0 179 if(pc_command == 'w') { //前進
yuki0701 0:9b75a5e505d0 180 m1+=500;
yuki0701 0:9b75a5e505d0 181 m2+=500;
yuki0701 0:9b75a5e505d0 182 m3-=500;
yuki0701 0:9b75a5e505d0 183 m4-=500;
yuki0701 0:9b75a5e505d0 184 } else if(pc_command == 's') { //後進
yuki0701 0:9b75a5e505d0 185 m1-=500;
yuki0701 0:9b75a5e505d0 186 m2-=500;
yuki0701 0:9b75a5e505d0 187 m3+=500;
yuki0701 0:9b75a5e505d0 188 m4+=500;
yuki0701 0:9b75a5e505d0 189 } else if(pc_command == 'd') { //右回り
yuki0701 0:9b75a5e505d0 190 m1+=500;
yuki0701 0:9b75a5e505d0 191 m2+=500;
yuki0701 0:9b75a5e505d0 192 m3+=500;
yuki0701 0:9b75a5e505d0 193 m4+=500;
yuki0701 0:9b75a5e505d0 194 } else if(pc_command == 'a') { //左回り
yuki0701 0:9b75a5e505d0 195 m1-=500;
yuki0701 0:9b75a5e505d0 196 m2-=500;
yuki0701 0:9b75a5e505d0 197 m3-=500;
yuki0701 0:9b75a5e505d0 198 m4-=500;
yuki0701 0:9b75a5e505d0 199 } else {
yuki0701 0:9b75a5e505d0 200 m1=0;
yuki0701 0:9b75a5e505d0 201 m2=0;
yuki0701 0:9b75a5e505d0 202 m3=0;
yuki0701 0:9b75a5e505d0 203 m4=0;
yuki0701 0:9b75a5e505d0 204 }
yuki0701 0:9b75a5e505d0 205
yuki0701 0:9b75a5e505d0 206 if(m1>4095) { //最大値を超えないように
yuki0701 0:9b75a5e505d0 207 m1=4095;
yuki0701 0:9b75a5e505d0 208 } else if(m1<-4095) {
yuki0701 0:9b75a5e505d0 209 m1=-4095;
yuki0701 0:9b75a5e505d0 210 }
yuki0701 0:9b75a5e505d0 211 if(m2>4095) {
yuki0701 0:9b75a5e505d0 212 m2=4095;
yuki0701 0:9b75a5e505d0 213 } else if(m2<-4095) {
yuki0701 0:9b75a5e505d0 214 m2=-4095;
yuki0701 0:9b75a5e505d0 215 }
yuki0701 0:9b75a5e505d0 216 if(m3>4095) {
yuki0701 0:9b75a5e505d0 217 m3=4095;
yuki0701 0:9b75a5e505d0 218 } else if(m3<-4095) {
yuki0701 0:9b75a5e505d0 219 m3=-4095;
yuki0701 0:9b75a5e505d0 220 }
yuki0701 0:9b75a5e505d0 221 if(m4>4095) {
yuki0701 0:9b75a5e505d0 222 m4=4095;
yuki0701 0:9b75a5e505d0 223 } else if(m4<-4095) {
yuki0701 0:9b75a5e505d0 224 m4=-4095;
yuki0701 0:9b75a5e505d0 225 }
yuki0701 0:9b75a5e505d0 226
yuki0701 0:9b75a5e505d0 227 debug_printf("%d %d %d %d\r\n",m1,m2,m3,m4);
yuki0701 0:9b75a5e505d0 228 MotorControl(m1,m2,m3,m4);
yuki0701 0:9b75a5e505d0 229 pc_command = '\0';
yuki0701 0:9b75a5e505d0 230 }
yuki0701 0:9b75a5e505d0 231 //#endif
yuki0701 0:9b75a5e505d0 232
yuki0701 0:9b75a5e505d0 233 //#ifdef DEBUG_PRINT
yuki0701 0:9b75a5e505d0 234 void debug_printf(const char* format,...)
yuki0701 0:9b75a5e505d0 235 {
yuki0701 0:9b75a5e505d0 236 va_list arg;
yuki0701 0:9b75a5e505d0 237 va_start(arg, format);
yuki0701 0:9b75a5e505d0 238 vprintf(format, arg);
yuki0701 0:9b75a5e505d0 239 va_end(arg);
yuki0701 0:9b75a5e505d0 240 }
yuki0701 0:9b75a5e505d0 241 //#endif