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

harurobo_mbed_undercarriage_sub

Maxon_setting.cpp@0:9b75a5e505d0, 2018-12-22 (annotated)

Committer:
yuki0701
Date:
Sat Dec 22 02:50:21 2018 +0000
Revision:
0:9b75a5e505d0
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yuki0701 0:9b75a5e505d0 1 #include <Maxon_setting.h>
yuki0701 0:9b75a5e505d0 2 //#include <mbed.h>
yuki0701 0:9b75a5e505d0 3 //#include <math.h>
yuki0701 0:9b75a5e505d0 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 //info.nowX = {0,0,0};
yuki0701 0:9b75a5e505d0 84 //info.nowY = {0,0,0};
yuki0701 0:9b75a5e505d0 85
yuki0701 0:9b75a5e505d0 86 //-----エスコンの初期設定-----//
yuki0701 0:9b75a5e505d0 87 spi.format(SPI_BITS, SPI_MODE);
yuki0701 0:9b75a5e505d0 88 spi.frequency(SPI_FREQ);
yuki0701 0:9b75a5e505d0 89 ss_md1 = 1;
yuki0701 0:9b75a5e505d0 90 ss_md2 = 1;
yuki0701 0:9b75a5e505d0 91 ss_md3 = 1;
yuki0701 0:9b75a5e505d0 92 ss_md4 = 1;
yuki0701 0:9b75a5e505d0 93 md_enable = 1; //enable on
yuki0701 0:9b75a5e505d0 94 md_err_led = 0;
yuki0701 0:9b75a5e505d0 95 md_stop = 1;
yuki0701 0:9b75a5e505d0 96 md_stop_led = 1;
yuki0701 0:9b75a5e505d0 97 //-----センサーの初期設定-----//
yuki0701 0:9b75a5e505d0 98 /* gyro.initialize();
yuki0701 0:9b75a5e505d0 99 motor_tick.attach(&calOmega,0.05); //0.05秒間隔で角速度を計算
yuki0701 0:9b75a5e505d0 100 EC1.setDiameter_mm(48);
yuki0701 0:9b75a5e505d0 101 EC2.setDiameter_mm(48); //測定輪半径//後で測定*/
yuki0701 0:9b75a5e505d0 102
yuki0701 0:9b75a5e505d0 103 #ifdef DEBUG_MODE
yuki0701 0:9b75a5e505d0 104 debug_led = 1;
yuki0701 0:9b75a5e505d0 105 pc.attach(Debug_Control, Serial::RxIrq);
yuki0701 0:9b75a5e505d0 106 #else
yuki0701 0:9b75a5e505d0 107 debug_led = 0;
yuki0701 0:9b75a5e505d0 108 #endif
yuki0701 0:9b75a5e505d0 109 }
yuki0701 0:9b75a5e505d0 110
yuki0701 0:9b75a5e505d0 111 #define MCP4922_AB (1<<15)
yuki0701 0:9b75a5e505d0 112 #define MCP4922_BUF (1<<14)
yuki0701 0:9b75a5e505d0 113 #define MCP4922_GA (1<<13)
yuki0701 0:9b75a5e505d0 114 #define MCP4922_SHDN (1<<12)
yuki0701 0:9b75a5e505d0 115
yuki0701 0:9b75a5e505d0 116 #define MCP4922_SET_OUTA (0x3000) //( MCP4922_GA || MCP4922_SHDN ) //12288
yuki0701 0:9b75a5e505d0 117 #define MCP4922_SET_OUTB (0xB000) //( MCP4922_AB || MCP4922_GA || MCP4922_SHDN ) //45056
yuki0701 0:9b75a5e505d0 118 #define MCP4922_MASKSET (0x0FFF) //4095
yuki0701 0:9b75a5e505d0 119
yuki0701 0:9b75a5e505d0 120 void DAC_Write(int16_t data, DigitalOut* DAC_cs)//(出力,出力場所)
yuki0701 0:9b75a5e505d0 121 {
yuki0701 0:9b75a5e505d0 122 static uint16_t dataA; //送るデータ
yuki0701 0:9b75a5e505d0 123 static uint16_t dataB;
yuki0701 0:9b75a5e505d0 124
yuki0701 0:9b75a5e505d0 125 dataA = MCP4922_SET_OUTA;
yuki0701 0:9b75a5e505d0 126 dataB = MCP4922_SET_OUTB;
yuki0701 0:9b75a5e505d0 127
yuki0701 0:9b75a5e505d0 128 if(data >= 0) {
yuki0701 0:9b75a5e505d0 129 if(data > 4095) {
yuki0701 0:9b75a5e505d0 130 data = 4095;
yuki0701 0:9b75a5e505d0 131 }
yuki0701 0:9b75a5e505d0 132 dataA += (MCP4922_MASKSET & (uint16_t)(data));
yuki0701 0:9b75a5e505d0 133 } else {
yuki0701 0:9b75a5e505d0 134 if(data < -4095) {
yuki0701 0:9b75a5e505d0 135 data = -4095;
yuki0701 0:9b75a5e505d0 136 }
yuki0701 0:9b75a5e505d0 137 dataB += (MCP4922_MASKSET & (uint16_t)(-data));
yuki0701 0:9b75a5e505d0 138 }
yuki0701 0:9b75a5e505d0 139
yuki0701 0:9b75a5e505d0 140 //Aの出力設定
yuki0701 0:9b75a5e505d0 141 (DigitalOut)(*DAC_cs)=0;
yuki0701 0:9b75a5e505d0 142 wait_us(SPI_WAIT_US);
yuki0701 0:9b75a5e505d0 143 spi.write(dataA);
yuki0701 0:9b75a5e505d0 144 wait_us(SPI_WAIT_US);
yuki0701 0:9b75a5e505d0 145 (DigitalOut)(*DAC_cs)=1;
yuki0701 0:9b75a5e505d0 146 wait_us(SPI_WAIT_US);
yuki0701 0:9b75a5e505d0 147
yuki0701 0:9b75a5e505d0 148 //Bの出力設定
yuki0701 0:9b75a5e505d0 149 (DigitalOut)(*DAC_cs)=0;
yuki0701 0:9b75a5e505d0 150 wait_us(SPI_WAIT_US);
yuki0701 0:9b75a5e505d0 151 spi.write(dataB);
yuki0701 0:9b75a5e505d0 152 wait_us(SPI_WAIT_US);
yuki0701 0:9b75a5e505d0 153 (DigitalOut)(*DAC_cs)=1;
yuki0701 0:9b75a5e505d0 154
yuki0701 0:9b75a5e505d0 155 }
yuki0701 0:9b75a5e505d0 156
yuki0701 0:9b75a5e505d0 157 void MotorControl(int16_t val_md1, int16_t val_md2, int16_t val_md3, int16_t val_md4) //出力
yuki0701 0:9b75a5e505d0 158 {
yuki0701 0:9b75a5e505d0 159 static int16_t zero_check;
yuki0701 0:9b75a5e505d0 160
yuki0701 0:9b75a5e505d0 161 DAC_Write(val_md1, &ss_md1);
yuki0701 0:9b75a5e505d0 162 DAC_Write(val_md2, &ss_md2);
yuki0701 0:9b75a5e505d0 163 DAC_Write(val_md3, &ss_md3);
yuki0701 0:9b75a5e505d0 164 DAC_Write(val_md4, &ss_md4);
yuki0701 0:9b75a5e505d0 165
yuki0701 0:9b75a5e505d0 166 zero_check = (val_md1 | val_md2 | val_md3 | val_md4); //すべての出力が0なら強制停止
yuki0701 0:9b75a5e505d0 167 if(zero_check == 0) {
yuki0701 0:9b75a5e505d0 168 md_stop = 1;
yuki0701 0:9b75a5e505d0 169 md_stop_led = 1;
yuki0701 0:9b75a5e505d0 170 } else {
yuki0701 0:9b75a5e505d0 171 md_stop = 0;
yuki0701 0:9b75a5e505d0 172 md_stop_led = 0;
yuki0701 0:9b75a5e505d0 173 }
yuki0701 0:9b75a5e505d0 174 }
yuki0701 0:9b75a5e505d0 175
yuki0701 0:9b75a5e505d0 176 //#ifdef DEBUG_MODE
yuki0701 0:9b75a5e505d0 177 void Debug_Control()
yuki0701 0:9b75a5e505d0 178 {
yuki0701 0:9b75a5e505d0 179 static char pc_command = '\0';
yuki0701 0:9b75a5e505d0 180
yuki0701 0:9b75a5e505d0 181 pc_command = pc.getc();
yuki0701 0:9b75a5e505d0 182
yuki0701 0:9b75a5e505d0 183 if(pc_command == 'w') { //前進
yuki0701 0:9b75a5e505d0 184 m1+=500;
yuki0701 0:9b75a5e505d0 185 m2+=500;
yuki0701 0:9b75a5e505d0 186 m3-=500;
yuki0701 0:9b75a5e505d0 187 m4-=500;
yuki0701 0:9b75a5e505d0 188 } else if(pc_command == 's') { //後進
yuki0701 0:9b75a5e505d0 189 m1-=500;
yuki0701 0:9b75a5e505d0 190 m2-=500;
yuki0701 0:9b75a5e505d0 191 m3+=500;
yuki0701 0:9b75a5e505d0 192 m4+=500;
yuki0701 0:9b75a5e505d0 193 } else if(pc_command == 'd') { //右回り
yuki0701 0:9b75a5e505d0 194 m1+=500;
yuki0701 0:9b75a5e505d0 195 m2+=500;
yuki0701 0:9b75a5e505d0 196 m3+=500;
yuki0701 0:9b75a5e505d0 197 m4+=500;
yuki0701 0:9b75a5e505d0 198 } else if(pc_command == 'a') { //左回り
yuki0701 0:9b75a5e505d0 199 m1-=500;
yuki0701 0:9b75a5e505d0 200 m2-=500;
yuki0701 0:9b75a5e505d0 201 m3-=500;
yuki0701 0:9b75a5e505d0 202 m4-=500;
yuki0701 0:9b75a5e505d0 203 } else {
yuki0701 0:9b75a5e505d0 204 m1=0;
yuki0701 0:9b75a5e505d0 205 m2=0;
yuki0701 0:9b75a5e505d0 206 m3=0;
yuki0701 0:9b75a5e505d0 207 m4=0;
yuki0701 0:9b75a5e505d0 208 }
yuki0701 0:9b75a5e505d0 209
yuki0701 0:9b75a5e505d0 210 if(m1>4095) { //最大値を超えないように
yuki0701 0:9b75a5e505d0 211 m1=4095;
yuki0701 0:9b75a5e505d0 212 } else if(m1<-4095) {
yuki0701 0:9b75a5e505d0 213 m1=-4095;
yuki0701 0:9b75a5e505d0 214 }
yuki0701 0:9b75a5e505d0 215 if(m2>4095) {
yuki0701 0:9b75a5e505d0 216 m2=4095;
yuki0701 0:9b75a5e505d0 217 } else if(m2<-4095) {
yuki0701 0:9b75a5e505d0 218 m2=-4095;
yuki0701 0:9b75a5e505d0 219 }
yuki0701 0:9b75a5e505d0 220 if(m3>4095) {
yuki0701 0:9b75a5e505d0 221 m3=4095;
yuki0701 0:9b75a5e505d0 222 } else if(m3<-4095) {
yuki0701 0:9b75a5e505d0 223 m3=-4095;
yuki0701 0:9b75a5e505d0 224 }
yuki0701 0:9b75a5e505d0 225 if(m4>4095) {
yuki0701 0:9b75a5e505d0 226 m4=4095;
yuki0701 0:9b75a5e505d0 227 } else if(m4<-4095) {
yuki0701 0:9b75a5e505d0 228 m4=-4095;
yuki0701 0:9b75a5e505d0 229 }
yuki0701 0:9b75a5e505d0 230
yuki0701 0:9b75a5e505d0 231 debug_printf("%d %d %d %d\r\n",m1,m2,m3,m4);
yuki0701 0:9b75a5e505d0 232 MotorControl(m1,m2,m3,m4);
yuki0701 0:9b75a5e505d0 233 pc_command = '\0';
yuki0701 0:9b75a5e505d0 234 }
yuki0701 0:9b75a5e505d0 235 //#endif
yuki0701 0:9b75a5e505d0 236
yuki0701 0:9b75a5e505d0 237 //#ifdef DEBUG_PRINT
yuki0701 0:9b75a5e505d0 238 void debug_printf(const char* format,...)
yuki0701 0:9b75a5e505d0 239 {
yuki0701 0:9b75a5e505d0 240 va_list arg;
yuki0701 0:9b75a5e505d0 241 va_start(arg, format);
yuki0701 0:9b75a5e505d0 242 vprintf(format, arg);
yuki0701 0:9b75a5e505d0 243 va_end(arg);
yuki0701 0:9b75a5e505d0 244 }
yuki0701 0:9b75a5e505d0 245 //#endif