春ロボ1班(元F3RC4班+) / Mbed 2 deprecated harurobo_ashi_3_2

3/2

Dependencies:   mbed move4wheel2 EC CruizCore_R1370P

maxonsetting/maxonsetting.cpp@0:c61c6e4775ca, 2019-02-13 (annotated)

Committer:
la00noix
Date:
Wed Feb 13 03:02:19 2019 +0000
Revision:
0:c61c6e4775ca
Child:
3:8a0faa3b08c3
a

Who changed what in which revision?

UserRevisionLine numberNew 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
la00noix 0:c61c6e4775ca 26 DigitalOut ss_md1(p15); //エスコンの設定
la00noix 0:c61c6e4775ca 27 DigitalOut ss_md2(p16);
la00noix 0:c61c6e4775ca 28 DigitalOut ss_md3(p17);
la00noix 0:c61c6e4775ca 29 DigitalOut ss_md4(p18);
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 }