diff --git a/.DS_Store b/.DS_Store deleted file mode 100644 index fcedf66..0000000 Binary files a/.DS_Store and /dev/null differ diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..1db9fb9 --- /dev/null +++ b/.clang-format @@ -0,0 +1,17 @@ +Language: Cpp +BasedOnStyle: Google + +AccessModifierOffset: -2 +AlignAfterOpenBracket: AlwaysBreak +BraceWrapping: + AfterClass: true + AfterFunction: true + AfterNamespace: true + AfterStruct: true +BreakBeforeBraces: Custom +ColumnLimit: 100 +ConstructorInitializerIndentWidth: 0 +ContinuationIndentWidth: 2 +DerivePointerAlignment: false +PointerAlignment: Middle +ReflowComments: false diff --git a/.github/workflows/compile-sketches.yaml b/.github/workflows/compile-sketches.yaml new file mode 100644 index 0000000..5755cc5 --- /dev/null +++ b/.github/workflows/compile-sketches.yaml @@ -0,0 +1,71 @@ +name: Compile Sketches + +on: + push: + paths-ignore: + - '**.md' + pull_request: + paths-ignore: + - '**.md' + workflow_dispatch: + +env: + # It's convenient to set variables for values used multiple times in the workflow + SKETCHES_REPORTS_PATH: sketches-reports + SKETCHES_REPORTS_ARTIFACT_NAME: sketches-reports + +jobs: + compile-sketches: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + - uses: arduino/compile-sketches@v1 + with: + enable-deltas-report: true + sketches-report-path: ${{ env.SKETCHES_REPORTS_PATH }} + github-token: ${{ secrets.GITHUB_TOKEN }} + fqbn: esp32:esp32:esp32s3 + platforms: | # ESP32公式のpackage indexを使用する + - name: esp32:esp32 + source-url: https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_index.json + version: 3.1.3 + libraries: | # 依存パッケージを指定 + - name: AsyncTCP + source-url: https://github.com/ESP32Async/AsyncTCP.git + - name: ESPAsyncWebServer + source-url: https://github.com/ESP32Async/ESPAsyncWebServer.git + sketch-paths: | + - pico_classic_v4_STEP1_LED + - pico_classic_v4_STEP2_SWITCH + - pico_classic_v4_STEP3_Buzzer + - pico_classic_v4_STEP4_Sensor + - pico_classic_v4_STEP5_Straight + - pico_classic_v4_STEP6_rotate + - pico_classic_v4_STEP7_P_control + - pico_classic_v4_STEP8_micromouse + + # This step is needed to pass the size data to the report job + - name: Upload sketches report to workflow artifact + uses: actions/upload-artifact@v4 + with: + name: ${{ env.SKETCHES_REPORTS_ARTIFACT_NAME }} + path: ${{ env.SKETCHES_REPORTS_PATH }} + + report: + needs: compile-sketches # Wait for the compile job to finish to get the data for the report + if: github.event_name == 'pull_request' # Only run the job when the workflow is triggered by a pull request + runs-on: ubuntu-latest + steps: + # This step is needed to get the size data produced by the compile jobs + - name: Download sketches reports artifact + uses: actions/download-artifact@v4 + with: + name: ${{ env.SKETCHES_REPORTS_ARTIFACT_NAME }} + path: ${{ env.SKETCHES_REPORTS_PATH }} + continue-on-error: true + + - uses: arduino/report-size-deltas@v1 + with: + sketches-reports-source: ${{ env.SKETCHES_REPORTS_PATH }} + github-token: ${{ secrets.GITHUB_TOKEN }} + continue-on-error: true diff --git a/.github/workflows/lint.yaml b/.github/workflows/lint.yaml new file mode 100644 index 0000000..0231117 --- /dev/null +++ b/.github/workflows/lint.yaml @@ -0,0 +1,30 @@ +name: Lint + +on: + push: + paths-ignore: + - '**.md' + pull_request: + paths-ignore: + - '**.md' + workflow_dispatch: + +jobs: + arduino-lint: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + - uses: arduino/arduino-lint-action@v2 + with: + recursive: true + compliance: specification + clang-format: + needs: arduino-lint + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + # 各スケッチファイルの.ino、.hファイルに対してclang-formatによる整形が必要か判定する + # 正規表現を簡単にするためzshを使用する + - run: sudo apt install -y clang-format zsh + - run: clang-format --dry-run -Werror pico_classic_v4_STEP*/*.(ino|h) + shell: zsh {0} diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..3691ca0 --- /dev/null +++ b/.gitignore @@ -0,0 +1,116 @@ +# Ref: https://github.com/arduino/Arduino/blob/master/.gitignore + +app/bin/ +app/pde.jar +build/macosx/work/ +arduino-core/bin/ +arduino-core/arduino-core.jar +hardware/arduino/bootloaders/caterina_LUFA/Descriptors.o +hardware/arduino/bootloaders/caterina_LUFA/Descriptors.lst +hardware/arduino/bootloaders/caterina_LUFA/Caterina.sym +hardware/arduino/bootloaders/caterina_LUFA/Caterina.o +hardware/arduino/bootloaders/caterina_LUFA/Caterina.map +hardware/arduino/bootloaders/caterina_LUFA/Caterina.lst +hardware/arduino/bootloaders/caterina_LUFA/Caterina.lss +hardware/arduino/bootloaders/caterina_LUFA/Caterina.elf +hardware/arduino/bootloaders/caterina_LUFA/Caterina.eep +hardware/arduino/bootloaders/caterina_LUFA/.dep/ +build/*.zip +build/*.tar.bz2 +build/windows/work/ +build/windows/*.zip +build/windows/*.tgz +build/windows/*.tar.bz2 +build/windows/libastylej* +build/windows/liblistSerials* +build/windows/arduino-*.zip +build/windows/dist/*.tar.gz +build/windows/dist/*.tar.bz2 +build/windows/launch4j-*.tgz +build/windows/launch4j-*.zip +build/windows/launcher/launch4j +build/windows/WinAVR-*.zip +build/macosx/arduino-*.zip +build/macosx/dist/*.tar.gz +build/macosx/dist/*.tar.bz2 +build/macosx/*.tar.bz2 +build/macosx/libastylej* +build/macosx/appbundler*.jar +build/macosx/appbundler*.zip +build/macosx/appbundler +build/macosx/appbundler-1.0ea-arduino? +build/macosx/appbundler-1.0ea-arduino*.zip +build/macosx/appbundler-1.0ea-upstream*.zip +build/linux/work/ +build/linux/dist/*.tar.gz +build/linux/dist/*.tar.bz2 +build/linux/*.tgz +build/linux/*.tar.xz +build/linux/*.tar.bz2 +build/linux/*.zip +build/linux/libastylej* +build/linux/liblistSerials* +build/shared/arduino-examples* +build/shared/reference*.zip +build/shared/Edison*.zip +build/shared/Galileo*.zip +build/shared/WiFi101-Updater-ArduinoIDE-Plugin*.zip +test-bin +*.iml +.idea +.DS_Store +.directory +hardware/arduino/avr/libraries/Bridge/examples/XivelyClient/passwords.h +avr-toolchain-*.zip +/app/nbproject/private/ +/arduino-core/nbproject/private/ +/app/build/ +/arduino-core/build/ + +manifest.mf +nbbuild.xml +nbproject + +# Ref: https://github.com/espressif/arduino-esp32/blob/master/.gitignore +tools/xtensa-esp32-elf +tools/xtensa-esp32s2-elf +tools/xtensa-esp32s3-elf +tools/riscv32-esp-elf +tools/dist +tools/esptool +tools/esptool.exe +tools/mkspiffs +tools/mklittlefs +tools/mkfatfs.exe +tools/openocd-esp32 + +# Ignore editor backup files and macOS system metadata +.DS_Store +.*.swp +.*.swo +*~ + +# Ignore build folder +/build + +# Ignore files built by Visual Studio/Visual Micro +[Dd]ebug/ +[Rr]elease/ +.vs/ +__vm/ +*.vcxproj* +.vscode/ +platform.sloeber.txt +boards.sloeber.txt + +# Ignore docs build (Sphinx) +docs/build +docs/source/_build +__pycache__/ +_build/ + +# Test log files +*.log +debug.cfg +debug.svd +debug_custom.json \ No newline at end of file diff --git a/README.md b/README.md index 1056101..0dfe4f4 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,8 @@ # pico_classic4_arduino_examples + +[![Compile Sketches](https://github.com/rt-net/pico_classic_v4_arduino_examples/actions/workflows/compile-sketches.yaml/badge.svg)](https://github.com/rt-net/pico_classic_v4_arduino_examples/actions/workflows/compile-sketches.yaml) +[![Lint](https://github.com/rt-net/pico_classic_v4_arduino_examples/actions/workflows/lint.yaml/badge.svg)](https://github.com/rt-net/pico_classic_v4_arduino_examples/actions/workflows/lint.yaml) + ![pico_classic4](images/PiCo_Classic4_image.jpg) Pi:Co Classic4用 Arduino サンプルスケッチ集です。 diff --git a/pico_classic_v4_STEP1_LED/.clang-format b/pico_classic_v4_STEP1_LED/.clang-format new file mode 120000 index 0000000..7cab60c --- /dev/null +++ b/pico_classic_v4_STEP1_LED/.clang-format @@ -0,0 +1 @@ +../.clang-format \ No newline at end of file diff --git a/pico_classic_v4_STEP1_LED/pico_classic_v4_STEP1_LED.ino b/pico_classic_v4_STEP1_LED/pico_classic_v4_STEP1_LED.ino old mode 100755 new mode 100644 index 582e28b..3c96610 --- a/pico_classic_v4_STEP1_LED/pico_classic_v4_STEP1_LED.ino +++ b/pico_classic_v4_STEP1_LED/pico_classic_v4_STEP1_LED.ino @@ -12,7 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. - #define LED0 13 #define LED1 14 #define LED2 47 diff --git a/pico_classic_v4_STEP2_SWITCH/.clang-format b/pico_classic_v4_STEP2_SWITCH/.clang-format new file mode 120000 index 0000000..7cab60c --- /dev/null +++ b/pico_classic_v4_STEP2_SWITCH/.clang-format @@ -0,0 +1 @@ +../.clang-format \ No newline at end of file diff --git a/pico_classic_v4_STEP2_SWITCH/pico_classic_v4_STEP2_SWITCH.ino b/pico_classic_v4_STEP2_SWITCH/pico_classic_v4_STEP2_SWITCH.ino old mode 100755 new mode 100644 index 1b37f57..2993344 --- a/pico_classic_v4_STEP2_SWITCH/pico_classic_v4_STEP2_SWITCH.ino +++ b/pico_classic_v4_STEP2_SWITCH/pico_classic_v4_STEP2_SWITCH.ino @@ -12,7 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. - #define LED0 13 #define LED1 14 #define LED2 47 @@ -37,7 +36,6 @@ void setup() pinMode(SW_R, INPUT_PULLUP); g_state_r = g_state_c = g_state_l = 0; - } void loop() @@ -51,7 +49,7 @@ void loop() } if (digitalRead(SW_C) == 0) { digitalWrite(LED1, (++g_state_c) & 0x01); - digitalWrite(LED2, (g_state_c)&0x01); + digitalWrite(LED2, (g_state_c) & 0x01); } if (digitalRead(SW_L) == 0) { digitalWrite(LED3, (++g_state_l) & 0x01); diff --git a/pico_classic_v4_STEP3_Buzzer/.clang-format b/pico_classic_v4_STEP3_Buzzer/.clang-format new file mode 120000 index 0000000..7cab60c --- /dev/null +++ b/pico_classic_v4_STEP3_Buzzer/.clang-format @@ -0,0 +1 @@ +../.clang-format \ No newline at end of file diff --git a/pico_classic_v4_STEP3_Buzzer/pico_classic_v4_STEP3_Buzzer.ino b/pico_classic_v4_STEP3_Buzzer/pico_classic_v4_STEP3_Buzzer.ino old mode 100755 new mode 100644 index 9c8a8ec..1b58de4 --- a/pico_classic_v4_STEP3_Buzzer/pico_classic_v4_STEP3_Buzzer.ino +++ b/pico_classic_v4_STEP3_Buzzer/pico_classic_v4_STEP3_Buzzer.ino @@ -12,7 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. - #define LED0 13 #define LED1 14 #define LED2 47 diff --git a/pico_classic_v4_STEP4_Sensor/.clang-format b/pico_classic_v4_STEP4_Sensor/.clang-format new file mode 120000 index 0000000..7cab60c --- /dev/null +++ b/pico_classic_v4_STEP4_Sensor/.clang-format @@ -0,0 +1 @@ +../.clang-format \ No newline at end of file diff --git a/pico_classic_v4_STEP4_Sensor/pico_classic_v4_STEP4_Sensor.ino b/pico_classic_v4_STEP4_Sensor/pico_classic_v4_STEP4_Sensor.ino old mode 100755 new mode 100644 index 4580c9d..ef0ee8c --- a/pico_classic_v4_STEP4_Sensor/pico_classic_v4_STEP4_Sensor.ino +++ b/pico_classic_v4_STEP4_Sensor/pico_classic_v4_STEP4_Sensor.ino @@ -12,7 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. - #define SLED_FR 9 #define SLED_FL 10 #define SLED_R 11 @@ -30,10 +29,11 @@ volatile short g_sensor_value_r; volatile short g_sensor_value_l; volatile short g_battery_value; -hw_timer_t* g_timer1 = NULL; +hw_timer_t * g_timer1 = NULL; portMUX_TYPE g_timer_mux = portMUX_INITIALIZER_UNLOCKED; -void IRAM_ATTR onTimer1(void) { +void IRAM_ATTR onTimer1(void) +{ static char cnt = 0; portENTER_CRITICAL_ISR(&g_timer_mux); switch (cnt) { @@ -79,7 +79,8 @@ void IRAM_ATTR onTimer1(void) { portEXIT_CRITICAL_ISR(&g_timer_mux); } -void setup() { +void setup() +{ // put your setup code here, to run once: //Sensor 発光off pinMode(SLED_FR, OUTPUT); @@ -99,7 +100,8 @@ void setup() { timerStart(g_timer1); } -void loop() { +void loop() +{ // put your main code here, to run repeatedly: Serial.printf("r_sen is %d\n\r", g_sensor_value_r); Serial.printf("fr_sen is %d\n\r", g_sensor_value_fr); diff --git a/pico_classic_v4_STEP5_Straight/.clang-format b/pico_classic_v4_STEP5_Straight/.clang-format new file mode 120000 index 0000000..7cab60c --- /dev/null +++ b/pico_classic_v4_STEP5_Straight/.clang-format @@ -0,0 +1 @@ +../.clang-format \ No newline at end of file diff --git a/pico_classic_v4_STEP5_Straight/TMC5240.h b/pico_classic_v4_STEP5_Straight/TMC5240.h index ba3cc47..30405cf 100644 --- a/pico_classic_v4_STEP5_Straight/TMC5240.h +++ b/pico_classic_v4_STEP5_Straight/TMC5240.h @@ -104,19 +104,15 @@ #define TMC5240_PULSE (TIRE_DIAMETER * PI / (200.0 * microstep)) #define TMC5240_VELOCITY (TMC5240_PULSE * 0.787) //13200000/2/2^23=0.787 +50c - -class TMC5240 { +class TMC5240 +{ private: - public: unsigned int readXactual(void); void write(unsigned char add, unsigned int data_l, unsigned int data_r); void init(void); }; - extern TMC5240 g_tmc5240; - - #endif // TMC5240_H_ diff --git a/pico_classic_v4_STEP5_Straight/TMC5240.ino b/pico_classic_v4_STEP5_Straight/TMC5240.ino index f4b58e0..5a8d741 100644 --- a/pico_classic_v4_STEP5_Straight/TMC5240.ino +++ b/pico_classic_v4_STEP5_Straight/TMC5240.ino @@ -17,7 +17,8 @@ TMC5240 g_tmc5240; portMUX_TYPE timerMux2 = portMUX_INITIALIZER_UNLOCKED; -unsigned int TMC5240::readXactual(void) { +unsigned int TMC5240::readXactual(void) +{ unsigned char spi_write_read_data[10]; unsigned int data_r, data_l; @@ -47,7 +48,9 @@ unsigned int TMC5240::readXactual(void) { digitalWrite(SPI_CS_L, HIGH); SPI.endTransaction(); - data_l = ((unsigned int)spi_write_read_data[1] << 24) | ((unsigned int)spi_write_read_data[2] << 16) | ((unsigned int)spi_write_read_data[3] << 8) | ((unsigned int)spi_write_read_data[4]); + data_l = ((unsigned int)spi_write_read_data[1] << 24) | + ((unsigned int)spi_write_read_data[2] << 16) | + ((unsigned int)spi_write_read_data[3] << 8) | ((unsigned int)spi_write_read_data[4]); for (int i = 0; i < 0xff; i++) { asm("nop \n"); @@ -78,13 +81,15 @@ unsigned int TMC5240::readXactual(void) { SPI.endTransaction(); portEXIT_CRITICAL_ISR(&timerMux2); - data_r = ((unsigned int)spi_write_read_data[1] << 24) | ((unsigned int)spi_write_read_data[2] << 16) | ((unsigned int)spi_write_read_data[3] << 8) | ((unsigned int)spi_write_read_data[4]); + data_r = ((unsigned int)spi_write_read_data[1] << 24) | + ((unsigned int)spi_write_read_data[2] << 16) | + ((unsigned int)spi_write_read_data[3] << 8) | ((unsigned int)spi_write_read_data[4]); return (unsigned int)(abs((int)data_r) + abs((int)data_l)); } -void TMC5240::write(unsigned char add, unsigned int data_l, unsigned int data_r) { - +void TMC5240::write(unsigned char add, unsigned int data_l, unsigned int data_r) +{ unsigned char spi_write_data[10]; portENTER_CRITICAL_ISR(&timerMux2); @@ -114,24 +119,31 @@ void TMC5240::write(unsigned char add, unsigned int data_l, unsigned int data_r) portEXIT_CRITICAL_ISR(&timerMux2); } -void TMC5240::init(void) { +void TMC5240::init(void) +{ SPI.begin(SPI_CLK, SPI_MISO, SPI_MOSI); - pinMode(SPI_CS_L, OUTPUT); //left + pinMode(SPI_CS_L, OUTPUT); //left pinMode(SPI_CS_R, OUTPUT); //right pinMode(SPI_CS_J, OUTPUT); digitalWrite(SPI_CS_L, HIGH); digitalWrite(SPI_CS_R, HIGH); digitalWrite(SPI_CS_J, HIGH); - g_tmc5240.write(TMC5240_DRV_CONF, 0x00000031, 0x00000031); //Current Range 2A 800V/us - g_tmc5240.write(TMC5240_IHOLD_IRUN, 0x01041C03, 0x01041C03); //IHOLDDELAY=4 IRUN=28/32 IHOLD=3/32 2A設定時ピーク1.5A、実効値1.06A - g_tmc5240.write(TMC5240_CHOPCONG, 0x04000001, 0x04000001); //MRES=0 1/16step TOFFTime=1 1以上でないとモータが動作しない - g_tmc5240.write(TMC5240_PWMCONF, 0xC40E001D, 0xC40E001D); //PWM 48.8kHz - g_tmc5240.write(TMC5240_RAMPMODE, 0x00000001, 0x00000001); //velocity mode(positive) - g_tmc5240.write(TMC5240_GCONF, 0x00000000, 0x00000010); //右のみシャフトインバース - g_tmc5240.write(TMC5240_AMAX, 0x3ffff, 0x3ffff); //加速度max 17..0 18bit - g_tmc5240.write(TMC5240_XACTUAL, 0, 0); //初期化 - g_tmc5240.write(TMC5240_VSTART, (unsigned int)(MIN_SPEED / TMC5240_VELOCITY), (unsigned int)(MIN_SPEED / TMC5240_VELOCITY)); + g_tmc5240.write(TMC5240_DRV_CONF, 0x00000031, 0x00000031); //Current Range 2A 800V/us + g_tmc5240.write( + TMC5240_IHOLD_IRUN, 0x01041C03, + 0x01041C03); //IHOLDDELAY=4 IRUN=28/32 IHOLD=3/32 2A設定時ピーク1.5A、実効値1.06A + g_tmc5240.write( + TMC5240_CHOPCONG, 0x04000001, + 0x04000001); //MRES=0 1/16step TOFFTime=1 1以上でないとモータが動作しない + g_tmc5240.write(TMC5240_PWMCONF, 0xC40E001D, 0xC40E001D); //PWM 48.8kHz + g_tmc5240.write(TMC5240_RAMPMODE, 0x00000001, 0x00000001); //velocity mode(positive) + g_tmc5240.write(TMC5240_GCONF, 0x00000000, 0x00000010); //右のみシャフトインバース + g_tmc5240.write(TMC5240_AMAX, 0x3ffff, 0x3ffff); //加速度max 17..0 18bit + g_tmc5240.write(TMC5240_XACTUAL, 0, 0); //初期化 + g_tmc5240.write( + TMC5240_VSTART, (unsigned int)(MIN_SPEED / TMC5240_VELOCITY), + (unsigned int)(MIN_SPEED / TMC5240_VELOCITY)); g_tmc5240.write(TMC5240_VMAX, 0, 0); } diff --git a/pico_classic_v4_STEP5_Straight/pico_classic_v4_STEP5_Straight.ino b/pico_classic_v4_STEP5_Straight/pico_classic_v4_STEP5_Straight.ino old mode 100755 new mode 100644 index 9f2a811..568cbfb --- a/pico_classic_v4_STEP5_Straight/pico_classic_v4_STEP5_Straight.ino +++ b/pico_classic_v4_STEP5_Straight/pico_classic_v4_STEP5_Straight.ino @@ -30,7 +30,7 @@ #define SPI_CLK 39 #define SPI_MOSI 42 #define SPI_MISO 41 -#define SPI_CS_L 40 //左モータ +#define SPI_CS_L 40 //左モータ #define SPI_CS_R 3 //右モータ #define SPI_CS_J 46 //ジャイロ @@ -39,21 +39,21 @@ #define MIN_SPEED 30 -hw_timer_t* g_timer0 = NULL; +hw_timer_t * g_timer0 = NULL; portMUX_TYPE g_timer_mux = portMUX_INITIALIZER_UNLOCKED; - //割り込み //目標値の更新周期1kHz -void IRAM_ATTR onTimer0(void) { +void IRAM_ATTR onTimer0(void) +{ portENTER_CRITICAL_ISR(&g_timer_mux); //割り込み禁止 controlInterrupt(); portEXIT_CRITICAL_ISR(&g_timer_mux); //割り込み許可 } - -void setup() { +void setup() +{ // put your setup code here, to run once: pinMode(LED0, OUTPUT); pinMode(LED1, OUTPUT); @@ -77,10 +77,10 @@ void setup() { delay(1); g_tmc5240.init(); digitalWrite(MOTOR_EN, HIGH); - } -void loop() { +void loop() +{ // put your main code here, to run repeatedly: while (digitalRead(SW_L) & digitalRead(SW_C) & digitalRead(SW_R)) { continue; diff --git a/pico_classic_v4_STEP5_Straight/run.h b/pico_classic_v4_STEP5_Straight/run.h old mode 100755 new mode 100644 index 1993b6a..124874d --- a/pico_classic_v4_STEP5_Straight/run.h +++ b/pico_classic_v4_STEP5_Straight/run.h @@ -20,16 +20,15 @@ typedef enum { MOT_BACK = 2 } t_CW_CCW; -class RUN { +class RUN +{ private: - public: volatile double accel; volatile double speed; volatile double max_speed; volatile double min_speed; - RUN(); void interrupt(void); void counterClear(void); @@ -42,10 +41,9 @@ class RUN { void decelerate(int len, int init_speed); private: - int step_lr_len,step_lr; + int step_lr_len, step_lr; }; - extern RUN g_run; #endif /* SRC_RUN_H_ */ diff --git a/pico_classic_v4_STEP5_Straight/run.ino b/pico_classic_v4_STEP5_Straight/run.ino old mode 100755 new mode 100644 index 2d7ac2c..8a711d5 --- a/pico_classic_v4_STEP5_Straight/run.ino +++ b/pico_classic_v4_STEP5_Straight/run.ino @@ -16,17 +16,17 @@ RUN g_run; -RUN::RUN() { +RUN::RUN() +{ speed = 0.0; accel = 0.0; } //割り込み -void controlInterrupt(void) { - g_run.interrupt(); -} +void controlInterrupt(void) { g_run.interrupt(); } -void RUN::interrupt(void) { //割り込み内からコール +void RUN::interrupt(void) +{ //割り込み内からコール speed += accel; @@ -38,35 +38,37 @@ void RUN::interrupt(void) { //割り込み内からコール } } -void RUN::dirSet(t_CW_CCW dir_left, t_CW_CCW dir_right) { +void RUN::dirSet(t_CW_CCW dir_left, t_CW_CCW dir_right) +{ g_tmc5240.write(TMC5240_RAMPMODE, dir_left, dir_right); } -void RUN::counterClear(void) { - g_tmc5240.write(TMC5240_XACTUAL, 0, 0); -} +void RUN::counterClear(void) { g_tmc5240.write(TMC5240_XACTUAL, 0, 0); } -void RUN::speedSet(double l_speed, double r_speed) { - g_tmc5240.write(TMC5240_VMAX, (unsigned int)(l_speed / TMC5240_VELOCITY), (unsigned int)(r_speed / TMC5240_VELOCITY)); +void RUN::speedSet(double l_speed, double r_speed) +{ + g_tmc5240.write( + TMC5240_VMAX, (unsigned int)(l_speed / TMC5240_VELOCITY), + (unsigned int)(r_speed / TMC5240_VELOCITY)); } -void RUN::stepGet(void) { +void RUN::stepGet(void) +{ step_lr = g_tmc5240.readXactual(); step_lr_len = (int)((float)step_lr / 2.0 * PULSE); } -void RUN::stop(void) { - g_tmc5240.write(TMC5240_VMAX, 0, 0); -} +void RUN::stop(void) { g_tmc5240.write(TMC5240_VMAX, 0, 0); } -void RUN::accelerate(int len, int finish_speed) { +void RUN::accelerate(int len, int finish_speed) +{ int obj_step; accel = 1.5; speed = min_speed = MIN_SPEED; max_speed = finish_speed; counterClear(); - speedSet(MIN_SPEED,MIN_SPEED); + speedSet(MIN_SPEED, MIN_SPEED); dirSet(MOT_FORWARD, MOT_FORWARD); obj_step = (int)((float)len * 2.0 / PULSE); @@ -79,14 +81,15 @@ void RUN::accelerate(int len, int finish_speed) { } } -void RUN::oneStep(int len, int init_speed) { +void RUN::oneStep(int len, int init_speed) +{ int obj_step; accel = 0.0; max_speed = init_speed; speed = min_speed = init_speed; counterClear(); - speedSet(init_speed,init_speed); + speedSet(init_speed, init_speed); dirSet(MOT_FORWARD, MOT_FORWARD); obj_step = (int)((float)len * 2.0 / PULSE); @@ -99,21 +102,24 @@ void RUN::oneStep(int len, int init_speed) { } } -void RUN::decelerate(int len, int init_speed) { +void RUN::decelerate(int len, int init_speed) +{ int obj_step; accel = 1.5; max_speed = init_speed; speed = min_speed = init_speed; counterClear(); - speedSet(init_speed,init_speed); + speedSet(init_speed, init_speed); dirSet(MOT_FORWARD, MOT_FORWARD); obj_step = (int)((float)len * 2.0 / PULSE); while (1) { stepGet(); speedSet(speed, speed); - if ((int)(len - step_lr_len) < (int)(((speed * speed) - (MIN_SPEED * MIN_SPEED)) / (2.0 * 1000.0 * accel))) { + if ( + (int)(len - step_lr_len) < + (int)(((speed * speed) - (MIN_SPEED * MIN_SPEED)) / (2.0 * 1000.0 * accel))) { break; } } diff --git a/pico_classic_v4_STEP6_rotate/.clang-format b/pico_classic_v4_STEP6_rotate/.clang-format new file mode 120000 index 0000000..7cab60c --- /dev/null +++ b/pico_classic_v4_STEP6_rotate/.clang-format @@ -0,0 +1 @@ +../.clang-format \ No newline at end of file diff --git a/pico_classic_v4_STEP6_rotate/TMC5240.h b/pico_classic_v4_STEP6_rotate/TMC5240.h index ba3cc47..30405cf 100644 --- a/pico_classic_v4_STEP6_rotate/TMC5240.h +++ b/pico_classic_v4_STEP6_rotate/TMC5240.h @@ -104,19 +104,15 @@ #define TMC5240_PULSE (TIRE_DIAMETER * PI / (200.0 * microstep)) #define TMC5240_VELOCITY (TMC5240_PULSE * 0.787) //13200000/2/2^23=0.787 +50c - -class TMC5240 { +class TMC5240 +{ private: - public: unsigned int readXactual(void); void write(unsigned char add, unsigned int data_l, unsigned int data_r); void init(void); }; - extern TMC5240 g_tmc5240; - - #endif // TMC5240_H_ diff --git a/pico_classic_v4_STEP6_rotate/TMC5240.ino b/pico_classic_v4_STEP6_rotate/TMC5240.ino index 945f035..f4380a5 100644 --- a/pico_classic_v4_STEP6_rotate/TMC5240.ino +++ b/pico_classic_v4_STEP6_rotate/TMC5240.ino @@ -17,7 +17,8 @@ TMC5240 g_tmc5240; portMUX_TYPE timerMux2 = portMUX_INITIALIZER_UNLOCKED; -unsigned int TMC5240::readXactual(void) { +unsigned int TMC5240::readXactual(void) +{ unsigned char spi_write_read_data[10]; unsigned int data_r, data_l; @@ -47,7 +48,9 @@ unsigned int TMC5240::readXactual(void) { digitalWrite(SPI_CS_L, HIGH); SPI.endTransaction(); - data_l = ((unsigned int)spi_write_read_data[1] << 24) | ((unsigned int)spi_write_read_data[2] << 16) | ((unsigned int)spi_write_read_data[3] << 8) | ((unsigned int)spi_write_read_data[4]); + data_l = ((unsigned int)spi_write_read_data[1] << 24) | + ((unsigned int)spi_write_read_data[2] << 16) | + ((unsigned int)spi_write_read_data[3] << 8) | ((unsigned int)spi_write_read_data[4]); for (int i = 0; i < 0xff; i++) { asm("nop \n"); @@ -78,13 +81,15 @@ unsigned int TMC5240::readXactual(void) { SPI.endTransaction(); portEXIT_CRITICAL_ISR(&timerMux2); - data_r = ((unsigned int)spi_write_read_data[1] << 24) | ((unsigned int)spi_write_read_data[2] << 16) | ((unsigned int)spi_write_read_data[3] << 8) | ((unsigned int)spi_write_read_data[4]); + data_r = ((unsigned int)spi_write_read_data[1] << 24) | + ((unsigned int)spi_write_read_data[2] << 16) | + ((unsigned int)spi_write_read_data[3] << 8) | ((unsigned int)spi_write_read_data[4]); return (unsigned int)(abs((int)data_r) + abs((int)data_l)); } -void TMC5240::write(unsigned char add, unsigned int data_l, unsigned int data_r) { - +void TMC5240::write(unsigned char add, unsigned int data_l, unsigned int data_r) +{ unsigned char spi_write_data[10]; portENTER_CRITICAL_ISR(&timerMux2); @@ -114,24 +119,31 @@ void TMC5240::write(unsigned char add, unsigned int data_l, unsigned int data_r) portEXIT_CRITICAL_ISR(&timerMux2); } -void TMC5240::init(void) { +void TMC5240::init(void) +{ SPI.begin(SPI_CLK, SPI_MISO, SPI_MOSI); - pinMode(SPI_CS_L, OUTPUT); //left + pinMode(SPI_CS_L, OUTPUT); //left pinMode(SPI_CS_R, OUTPUT); //right pinMode(SPI_CS_J, OUTPUT); digitalWrite(SPI_CS_L, HIGH); digitalWrite(SPI_CS_R, HIGH); digitalWrite(SPI_CS_J, HIGH); - g_tmc5240.write(TMC5240_DRV_CONF, 0x00000031, 0x00000031); //Current Range 2A 800V/us - g_tmc5240.write(TMC5240_IHOLD_IRUN, 0x01041C03, 0x01041C03); //IHOLDDELAY=4 IRUN=28/32 IHOLD=3/32 2A設定時 28=1.2A(RMS)、3=0.132A(RMS) - g_tmc5240.write(TMC5240_CHOPCONG, 0x04000001, 0x04000001); //MRES=0 1/16step TOFFTime=1 1以上でないとモータが動作しない - g_tmc5240.write(TMC5240_PWMCONF, 0xC40E001D, 0xC40E001D); //PWM 48.8kHz - g_tmc5240.write(TMC5240_RAMPMODE, 0x00000001, 0x00000001); //velocity mode(positive) - g_tmc5240.write(TMC5240_GCONF, 0x00000000, 0x00000010); //右のみシャフトインバース - g_tmc5240.write(TMC5240_AMAX, 0x3ffff, 0x3ffff); //加速度max 17..0 18bit - g_tmc5240.write(TMC5240_XACTUAL, 0, 0); //初期化 - g_tmc5240.write(TMC5240_VSTART, (unsigned int)(MIN_SPEED / TMC5240_VELOCITY), (unsigned int)(MIN_SPEED / TMC5240_VELOCITY)); + g_tmc5240.write(TMC5240_DRV_CONF, 0x00000031, 0x00000031); //Current Range 2A 800V/us + g_tmc5240.write( + TMC5240_IHOLD_IRUN, 0x01041C03, + 0x01041C03); //IHOLDDELAY=4 IRUN=28/32 IHOLD=3/32 2A設定時 28=1.2A(RMS)、3=0.132A(RMS) + g_tmc5240.write( + TMC5240_CHOPCONG, 0x04000001, + 0x04000001); //MRES=0 1/16step TOFFTime=1 1以上でないとモータが動作しない + g_tmc5240.write(TMC5240_PWMCONF, 0xC40E001D, 0xC40E001D); //PWM 48.8kHz + g_tmc5240.write(TMC5240_RAMPMODE, 0x00000001, 0x00000001); //velocity mode(positive) + g_tmc5240.write(TMC5240_GCONF, 0x00000000, 0x00000010); //右のみシャフトインバース + g_tmc5240.write(TMC5240_AMAX, 0x3ffff, 0x3ffff); //加速度max 17..0 18bit + g_tmc5240.write(TMC5240_XACTUAL, 0, 0); //初期化 + g_tmc5240.write( + TMC5240_VSTART, (unsigned int)(MIN_SPEED / TMC5240_VELOCITY), + (unsigned int)(MIN_SPEED / TMC5240_VELOCITY)); g_tmc5240.write(TMC5240_VMAX, 0, 0); } diff --git a/pico_classic_v4_STEP6_rotate/pico_classic_v4_STEP6_rotate.ino b/pico_classic_v4_STEP6_rotate/pico_classic_v4_STEP6_rotate.ino old mode 100755 new mode 100644 index 82abb9e..26034c0 --- a/pico_classic_v4_STEP6_rotate/pico_classic_v4_STEP6_rotate.ino +++ b/pico_classic_v4_STEP6_rotate/pico_classic_v4_STEP6_rotate.ino @@ -30,7 +30,7 @@ #define SPI_CLK 39 #define SPI_MOSI 42 #define SPI_MISO 41 -#define SPI_CS_L 40 //左モータ +#define SPI_CS_L 40 //左モータ #define SPI_CS_R 3 //右モータ #define SPI_CS_J 46 //ジャイロ @@ -40,21 +40,21 @@ #define MIN_SPEED 30 -hw_timer_t* g_timer0 = NULL; +hw_timer_t * g_timer0 = NULL; portMUX_TYPE g_timer_mux = portMUX_INITIALIZER_UNLOCKED; - //割り込み //目標値の更新周期1kHz -void IRAM_ATTR onTimer0(void) { +void IRAM_ATTR onTimer0(void) +{ portENTER_CRITICAL_ISR(&g_timer_mux); //割り込み禁止 controlInterrupt(); portEXIT_CRITICAL_ISR(&g_timer_mux); //割り込み許可 } - -void setup() { +void setup() +{ // put your setup code here, to run once: pinMode(LED0, OUTPUT); pinMode(LED1, OUTPUT); @@ -78,10 +78,10 @@ void setup() { delay(1); g_tmc5240.init(); digitalWrite(MOTOR_EN, HIGH); - } -void loop() { +void loop() +{ // put your main code here, to run repeatedly: while (digitalRead(SW_L) & digitalRead(SW_C) & digitalRead(SW_R)) { delay(1); diff --git a/pico_classic_v4_STEP6_rotate/run.h b/pico_classic_v4_STEP6_rotate/run.h old mode 100755 new mode 100644 index 8226c19..f16e5f1 --- a/pico_classic_v4_STEP6_rotate/run.h +++ b/pico_classic_v4_STEP6_rotate/run.h @@ -22,16 +22,15 @@ typedef enum { MOT_BACK = 2 } t_CW_CCW; -class RUN { +class RUN +{ private: - public: volatile double accel; volatile double speed; volatile double max_speed; volatile double min_speed; - RUN(); void interrupt(void); void counterClear(void); @@ -45,10 +44,9 @@ class RUN { void rotate(t_local_direction dir, int times); private: - int step_lr_len,step_lr; + int step_lr_len, step_lr; }; - extern RUN g_run; #endif /* SRC_RUN_H_ */ diff --git a/pico_classic_v4_STEP6_rotate/run.ino b/pico_classic_v4_STEP6_rotate/run.ino old mode 100755 new mode 100644 index 57377e8..cac2418 --- a/pico_classic_v4_STEP6_rotate/run.ino +++ b/pico_classic_v4_STEP6_rotate/run.ino @@ -16,17 +16,17 @@ RUN g_run; -RUN::RUN() { +RUN::RUN() +{ speed = 0.0; accel = 0.0; } //割り込み -void controlInterrupt(void) { - g_run.interrupt(); -} +void controlInterrupt(void) { g_run.interrupt(); } -void RUN::interrupt(void) { //割り込み内からコール +void RUN::interrupt(void) +{ //割り込み内からコール speed += accel; @@ -38,37 +38,38 @@ void RUN::interrupt(void) { //割り込み内からコール } } -void RUN::dirSet(t_CW_CCW dir_left, t_CW_CCW dir_right) { +void RUN::dirSet(t_CW_CCW dir_left, t_CW_CCW dir_right) +{ g_tmc5240.write(TMC5240_RAMPMODE, dir_left, dir_right); } -void RUN::counterClear(void) { - g_tmc5240.write(TMC5240_XACTUAL, 0, 0); -} +void RUN::counterClear(void) { g_tmc5240.write(TMC5240_XACTUAL, 0, 0); } -void RUN::speedSet(double l_speed, double r_speed) { - g_tmc5240.write(TMC5240_VMAX, (unsigned int)(l_speed / TMC5240_VELOCITY), (unsigned int)(r_speed / TMC5240_VELOCITY)); +void RUN::speedSet(double l_speed, double r_speed) +{ + g_tmc5240.write( + TMC5240_VMAX, (unsigned int)(l_speed / TMC5240_VELOCITY), + (unsigned int)(r_speed / TMC5240_VELOCITY)); } -void RUN::stepGet(void) { +void RUN::stepGet(void) +{ step_lr = g_tmc5240.readXactual(); step_lr_len = (int)((float)step_lr / 2.0 * PULSE); } -void RUN::stop(void) { - g_tmc5240.write(TMC5240_VMAX, 0, 0); -} - +void RUN::stop(void) { g_tmc5240.write(TMC5240_VMAX, 0, 0); } -void RUN::accelerate(int len, int finish_speed) { +void RUN::accelerate(int len, int finish_speed) +{ int obj_step; accel = 1.5; speed = min_speed = MIN_SPEED; max_speed = finish_speed; counterClear(); - speedSet(MIN_SPEED,MIN_SPEED); - dirSet(MOT_FORWARD, MOT_FORWARD); + speedSet(MIN_SPEED, MIN_SPEED); + dirSet(MOT_FORWARD, MOT_FORWARD); obj_step = (int)((float)len * 2.0 / PULSE); while (1) { stepGet(); @@ -79,14 +80,15 @@ void RUN::accelerate(int len, int finish_speed) { } } -void RUN::oneStep(int len, int init_speed) { +void RUN::oneStep(int len, int init_speed) +{ int obj_step; accel = 0.0; max_speed = init_speed; speed = min_speed = init_speed; counterClear(); - speedSet(init_speed,init_speed); + speedSet(init_speed, init_speed); dirSet(MOT_FORWARD, MOT_FORWARD); obj_step = (int)((float)len * 2.0 / PULSE); @@ -99,21 +101,24 @@ void RUN::oneStep(int len, int init_speed) { } } -void RUN::decelerate(int len, int init_speed) { +void RUN::decelerate(int len, int init_speed) +{ int obj_step; accel = 1.5; max_speed = init_speed; speed = min_speed = init_speed; counterClear(); - speedSet(init_speed,init_speed); - dirSet(MOT_FORWARD, MOT_FORWARD); + speedSet(init_speed, init_speed); + dirSet(MOT_FORWARD, MOT_FORWARD); obj_step = (int)((float)len * 2.0 / PULSE); while (1) { stepGet(); speedSet(speed, speed); - if ((int)(len - step_lr_len) < (int)(((speed * speed) - (MIN_SPEED * MIN_SPEED)) / (2.0 * 1000.0 * accel))) { + if ( + (int)(len - step_lr_len) < + (int)(((speed * speed) - (MIN_SPEED * MIN_SPEED)) / (2.0 * 1000.0 * accel))) { break; } } @@ -132,7 +137,6 @@ void RUN::decelerate(int len, int init_speed) { stop(); } - void RUN::rotate(t_local_direction dir, int times) { int obj_step; @@ -141,15 +145,15 @@ void RUN::rotate(t_local_direction dir, int times) max_speed = 350.0; speed = min_speed = MIN_SPEED; counterClear(); - speedSet(MIN_SPEED,MIN_SPEED); + speedSet(MIN_SPEED, MIN_SPEED); obj_step = (int)(TREAD_WIDTH * PI / 4.0 * (float)times * 2.0 / PULSE); switch (dir) { case right: - dirSet(MOT_FORWARD, MOT_BACK); + dirSet(MOT_FORWARD, MOT_BACK); break; case left: - dirSet(MOT_BACK,MOT_FORWARD); + dirSet(MOT_BACK, MOT_FORWARD); break; default: dirSet(MOT_FORWARD, MOT_FORWARD); @@ -159,7 +163,9 @@ void RUN::rotate(t_local_direction dir, int times) while (1) { stepGet(); speedSet(speed, speed); - if ((int)((obj_step/2.0*PULSE) - step_lr_len) < (int)(((speed * speed) - (MIN_SPEED * MIN_SPEED)) / (2.0 * 1000.0 * accel))) { + if ( + (int)((obj_step / 2.0 * PULSE) - step_lr_len) < + (int)(((speed * speed) - (MIN_SPEED * MIN_SPEED)) / (2.0 * 1000.0 * accel))) { break; } } @@ -176,5 +182,4 @@ void RUN::rotate(t_local_direction dir, int times) } stop(); - } diff --git a/pico_classic_v4_STEP7_P_control/.clang-format b/pico_classic_v4_STEP7_P_control/.clang-format new file mode 120000 index 0000000..7cab60c --- /dev/null +++ b/pico_classic_v4_STEP7_P_control/.clang-format @@ -0,0 +1 @@ +../.clang-format \ No newline at end of file diff --git a/pico_classic_v4_STEP7_P_control/TMC5240.h b/pico_classic_v4_STEP7_P_control/TMC5240.h index ba3cc47..30405cf 100644 --- a/pico_classic_v4_STEP7_P_control/TMC5240.h +++ b/pico_classic_v4_STEP7_P_control/TMC5240.h @@ -104,19 +104,15 @@ #define TMC5240_PULSE (TIRE_DIAMETER * PI / (200.0 * microstep)) #define TMC5240_VELOCITY (TMC5240_PULSE * 0.787) //13200000/2/2^23=0.787 +50c - -class TMC5240 { +class TMC5240 +{ private: - public: unsigned int readXactual(void); void write(unsigned char add, unsigned int data_l, unsigned int data_r); void init(void); }; - extern TMC5240 g_tmc5240; - - #endif // TMC5240_H_ diff --git a/pico_classic_v4_STEP7_P_control/TMC5240.ino b/pico_classic_v4_STEP7_P_control/TMC5240.ino index f359c87..5a8d741 100644 --- a/pico_classic_v4_STEP7_P_control/TMC5240.ino +++ b/pico_classic_v4_STEP7_P_control/TMC5240.ino @@ -17,7 +17,8 @@ TMC5240 g_tmc5240; portMUX_TYPE timerMux2 = portMUX_INITIALIZER_UNLOCKED; -unsigned int TMC5240::readXactual(void) { +unsigned int TMC5240::readXactual(void) +{ unsigned char spi_write_read_data[10]; unsigned int data_r, data_l; @@ -47,7 +48,9 @@ unsigned int TMC5240::readXactual(void) { digitalWrite(SPI_CS_L, HIGH); SPI.endTransaction(); - data_l = ((unsigned int)spi_write_read_data[1] << 24) | ((unsigned int)spi_write_read_data[2] << 16) | ((unsigned int)spi_write_read_data[3] << 8) | ((unsigned int)spi_write_read_data[4]); + data_l = ((unsigned int)spi_write_read_data[1] << 24) | + ((unsigned int)spi_write_read_data[2] << 16) | + ((unsigned int)spi_write_read_data[3] << 8) | ((unsigned int)spi_write_read_data[4]); for (int i = 0; i < 0xff; i++) { asm("nop \n"); @@ -78,13 +81,15 @@ unsigned int TMC5240::readXactual(void) { SPI.endTransaction(); portEXIT_CRITICAL_ISR(&timerMux2); - data_r = ((unsigned int)spi_write_read_data[1] << 24) | ((unsigned int)spi_write_read_data[2] << 16) | ((unsigned int)spi_write_read_data[3] << 8) | ((unsigned int)spi_write_read_data[4]); + data_r = ((unsigned int)spi_write_read_data[1] << 24) | + ((unsigned int)spi_write_read_data[2] << 16) | + ((unsigned int)spi_write_read_data[3] << 8) | ((unsigned int)spi_write_read_data[4]); return (unsigned int)(abs((int)data_r) + abs((int)data_l)); } -void TMC5240::write(unsigned char add, unsigned int data_l, unsigned int data_r) { - +void TMC5240::write(unsigned char add, unsigned int data_l, unsigned int data_r) +{ unsigned char spi_write_data[10]; portENTER_CRITICAL_ISR(&timerMux2); @@ -114,24 +119,31 @@ void TMC5240::write(unsigned char add, unsigned int data_l, unsigned int data_r) portEXIT_CRITICAL_ISR(&timerMux2); } -void TMC5240::init(void) { +void TMC5240::init(void) +{ SPI.begin(SPI_CLK, SPI_MISO, SPI_MOSI); - pinMode(SPI_CS_L, OUTPUT); //left + pinMode(SPI_CS_L, OUTPUT); //left pinMode(SPI_CS_R, OUTPUT); //right pinMode(SPI_CS_J, OUTPUT); digitalWrite(SPI_CS_L, HIGH); digitalWrite(SPI_CS_R, HIGH); digitalWrite(SPI_CS_J, HIGH); - g_tmc5240.write(TMC5240_DRV_CONF, 0x00000031, 0x00000031); //Current Range 2A 800V/us - g_tmc5240.write(TMC5240_IHOLD_IRUN, 0x01041C03, 0x01041C03); //IHOLDDELAY=4 IRUN=28/32 IHOLD=3/32 2A設定時ピーク1.5A、実効値1.06A - g_tmc5240.write(TMC5240_CHOPCONG, 0x04000001, 0x04000001); //MRES=0 1/16step TOFFTime=1 1以上でないとモータが動作しない - g_tmc5240.write(TMC5240_PWMCONF, 0xC40E001D, 0xC40E001D); //PWM 48.8kHz - g_tmc5240.write(TMC5240_RAMPMODE, 0x00000001, 0x00000001); //velocity mode(positive) - g_tmc5240.write(TMC5240_GCONF, 0x00000000, 0x00000010); //右のみシャフトインバース - g_tmc5240.write(TMC5240_AMAX, 0x3ffff, 0x3ffff); //加速度max 17..0 18bit - g_tmc5240.write(TMC5240_XACTUAL, 0, 0); //初期化 - g_tmc5240.write(TMC5240_VSTART, (unsigned int)(MIN_SPEED / TMC5240_VELOCITY), (unsigned int)(MIN_SPEED / TMC5240_VELOCITY)); + g_tmc5240.write(TMC5240_DRV_CONF, 0x00000031, 0x00000031); //Current Range 2A 800V/us + g_tmc5240.write( + TMC5240_IHOLD_IRUN, 0x01041C03, + 0x01041C03); //IHOLDDELAY=4 IRUN=28/32 IHOLD=3/32 2A設定時ピーク1.5A、実効値1.06A + g_tmc5240.write( + TMC5240_CHOPCONG, 0x04000001, + 0x04000001); //MRES=0 1/16step TOFFTime=1 1以上でないとモータが動作しない + g_tmc5240.write(TMC5240_PWMCONF, 0xC40E001D, 0xC40E001D); //PWM 48.8kHz + g_tmc5240.write(TMC5240_RAMPMODE, 0x00000001, 0x00000001); //velocity mode(positive) + g_tmc5240.write(TMC5240_GCONF, 0x00000000, 0x00000010); //右のみシャフトインバース + g_tmc5240.write(TMC5240_AMAX, 0x3ffff, 0x3ffff); //加速度max 17..0 18bit + g_tmc5240.write(TMC5240_XACTUAL, 0, 0); //初期化 + g_tmc5240.write( + TMC5240_VSTART, (unsigned int)(MIN_SPEED / TMC5240_VELOCITY), + (unsigned int)(MIN_SPEED / TMC5240_VELOCITY)); g_tmc5240.write(TMC5240_VMAX, 0, 0); } diff --git a/pico_classic_v4_STEP7_P_control/pico_classic_v4_STEP7_P_control.ino b/pico_classic_v4_STEP7_P_control/pico_classic_v4_STEP7_P_control.ino old mode 100755 new mode 100644 index c7b50fb..1e1e9f0 --- a/pico_classic_v4_STEP7_P_control/pico_classic_v4_STEP7_P_control.ino +++ b/pico_classic_v4_STEP7_P_control/pico_classic_v4_STEP7_P_control.ino @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "run.h" -#include "sensor.h" #include "SPI.h" #include "TMC5240.h" +#include "run.h" +#include "sensor.h" #define LED0 13 #define LED1 14 @@ -42,7 +42,7 @@ #define SPI_CLK 39 #define SPI_MOSI 42 #define SPI_MISO 41 -#define SPI_CS_L 40 //左モータ +#define SPI_CS_L 40 //左モータ #define SPI_CS_R 3 //右モータ #define SPI_CS_J 46 //ジャイロ @@ -52,7 +52,6 @@ #define MIN_SPEED 30 - //環境に合わせて変更する //ここから #define REF_SEN_R 352 @@ -69,14 +68,14 @@ #define CON_WALL_KP (0.5) //ここまで - hw_timer_t * g_timer0 = NULL; hw_timer_t * g_timer1 = NULL; portMUX_TYPE g_timer_mux = portMUX_INITIALIZER_UNLOCKED; //割り込み //目標値の更新周期1kHz -void IRAM_ATTR onTimer0(void) { +void IRAM_ATTR onTimer0(void) +{ portENTER_CRITICAL_ISR(&g_timer_mux); //割り込み禁止 controlInterrupt(); portEXIT_CRITICAL_ISR(&g_timer_mux); //割り込み許可 @@ -89,7 +88,8 @@ void IRAM_ATTR onTimer1(void) portEXIT_CRITICAL_ISR(&g_timer_mux); //割り込み許可 } -void setup() { +void setup() +{ // put your setup code here, to run once: pinMode(LED0, OUTPUT); pinMode(LED1, OUTPUT); @@ -129,10 +129,10 @@ void setup() { delay(1); g_tmc5240.init(); digitalWrite(MOTOR_EN, HIGH); - } -void loop() { +void loop() +{ // put your main code here, to run repeatedly: while (digitalRead(SW_L) & digitalRead(SW_C) & digitalRead(SW_R)) { continue; diff --git a/pico_classic_v4_STEP7_P_control/run.h b/pico_classic_v4_STEP7_P_control/run.h old mode 100755 new mode 100644 index d5e3a46..97c6bae --- a/pico_classic_v4_STEP7_P_control/run.h +++ b/pico_classic_v4_STEP7_P_control/run.h @@ -37,20 +37,19 @@ typedef enum { MOT_BACK = 2 } t_CW_CCW; -class RUN { +class RUN +{ private: - public: volatile double accel; volatile double speed; volatile double speed_target_r; - volatile double speed_target_l; + volatile double speed_target_l; volatile double max_speed; volatile double min_speed; t_control con_wall; - RUN(); void interrupt(void); void counterClear(void); @@ -64,10 +63,9 @@ class RUN { void rotate(t_local_direction dir, int times); private: - int step_lr_len,step_lr; + int step_lr_len, step_lr; }; - extern RUN g_run; #endif /* SRC_RUN_H_ */ diff --git a/pico_classic_v4_STEP7_P_control/run.ino b/pico_classic_v4_STEP7_P_control/run.ino old mode 100755 new mode 100644 index 3abf514..43e77e7 --- a/pico_classic_v4_STEP7_P_control/run.ino +++ b/pico_classic_v4_STEP7_P_control/run.ino @@ -17,18 +17,18 @@ RUN g_run; -RUN::RUN() { +RUN::RUN() +{ speed = 0.0; accel = 0.0; con_wall.kp = CON_WALL_KP; } //割り込み -void controlInterrupt(void) { - g_run.interrupt(); -} +void controlInterrupt(void) { g_run.interrupt(); } -void RUN::interrupt(void) { //割り込み内からコール +void RUN::interrupt(void) +{ //割り込み内からコール speed += accel; @@ -59,28 +59,30 @@ void RUN::interrupt(void) { //割り込み内からコール } } -void RUN::dirSet(t_CW_CCW dir_left, t_CW_CCW dir_right) { +void RUN::dirSet(t_CW_CCW dir_left, t_CW_CCW dir_right) +{ g_tmc5240.write(TMC5240_RAMPMODE, dir_left, dir_right); } -void RUN::counterClear(void) { - g_tmc5240.write(TMC5240_XACTUAL, 0, 0); -} +void RUN::counterClear(void) { g_tmc5240.write(TMC5240_XACTUAL, 0, 0); } -void RUN::speedSet(double l_speed, double r_speed) { - g_tmc5240.write(TMC5240_VMAX, (unsigned int)(l_speed / TMC5240_VELOCITY), (unsigned int)(r_speed / TMC5240_VELOCITY)); +void RUN::speedSet(double l_speed, double r_speed) +{ + g_tmc5240.write( + TMC5240_VMAX, (unsigned int)(l_speed / TMC5240_VELOCITY), + (unsigned int)(r_speed / TMC5240_VELOCITY)); } -void RUN::stepGet(void) { +void RUN::stepGet(void) +{ step_lr = g_tmc5240.readXactual(); step_lr_len = (int)((float)step_lr / 2.0 * PULSE); } -void RUN::stop(void) { - g_tmc5240.write(TMC5240_VMAX, 0, 0); -} +void RUN::stop(void) { g_tmc5240.write(TMC5240_VMAX, 0, 0); } -void RUN::accelerate(int len, int finish_speed) { +void RUN::accelerate(int len, int finish_speed) +{ int obj_step; accel = 1.5; @@ -99,7 +101,8 @@ void RUN::accelerate(int len, int finish_speed) { } } -void RUN::oneStep(int len, int init_speed) { +void RUN::oneStep(int len, int init_speed) +{ int obj_step; accel = 0.0; @@ -119,7 +122,8 @@ void RUN::oneStep(int len, int init_speed) { } } -void RUN::decelerate(int len, int init_speed) { +void RUN::decelerate(int len, int init_speed) +{ int obj_step; accel = 1.5; @@ -133,7 +137,9 @@ void RUN::decelerate(int len, int init_speed) { while (1) { stepGet(); speedSet(speed_target_l, speed_target_r); - if ((int)(len - step_lr_len) < (int)(((speed * speed) - (MIN_SPEED * MIN_SPEED)) / (2.0 * 1000.0 * accel))) { + if ( + (int)(len - step_lr_len) < + (int)(((speed * speed) - (MIN_SPEED * MIN_SPEED)) / (2.0 * 1000.0 * accel))) { break; } } @@ -152,8 +158,8 @@ void RUN::decelerate(int len, int init_speed) { stop(); } - -void RUN::rotate(t_local_direction dir, int times) { +void RUN::rotate(t_local_direction dir, int times) +{ int obj_step; accel = 1.5; @@ -176,7 +182,9 @@ void RUN::rotate(t_local_direction dir, int times) { while (1) { stepGet(); speedSet(speed, speed); - if ((int)((obj_step / 2.0 * PULSE) - step_lr_len) < (int)(((speed * speed) - (MIN_SPEED * MIN_SPEED)) / (2.0 * 1000.0 * accel))) { + if ( + (int)((obj_step / 2.0 * PULSE) - step_lr_len) < + (int)(((speed * speed) - (MIN_SPEED * MIN_SPEED)) / (2.0 * 1000.0 * accel))) { break; } } diff --git a/pico_classic_v4_STEP7_P_control/sensor.h b/pico_classic_v4_STEP7_P_control/sensor.h index 52a72c9..162523f 100644 --- a/pico_classic_v4_STEP7_P_control/sensor.h +++ b/pico_classic_v4_STEP7_P_control/sensor.h @@ -29,13 +29,13 @@ typedef struct bool is_control; } t_sensor; - -class SENSOR{ +class SENSOR +{ public: SENSOR(); - volatile t_sensor sen_r, sen_l, sen_fr, sen_fl; - volatile short battery_value; - void interrupt(void); + volatile t_sensor sen_r, sen_l, sen_fr, sen_fl; + volatile short battery_value; + void interrupt(void); }; extern SENSOR g_sensor; diff --git a/pico_classic_v4_STEP7_P_control/sensor.ino b/pico_classic_v4_STEP7_P_control/sensor.ino index 9fe0ca6..fd02d62 100644 --- a/pico_classic_v4_STEP7_P_control/sensor.ino +++ b/pico_classic_v4_STEP7_P_control/sensor.ino @@ -16,8 +16,8 @@ SENSOR g_sensor; - -SENSOR::SENSOR() { //コンストラクタ +SENSOR::SENSOR() +{ //コンストラクタ sen_r.ref = REF_SEN_R; sen_l.ref = REF_SEN_L; @@ -31,12 +31,9 @@ SENSOR::SENSOR() { //コンストラクタ sen_l.th_control = CONTH_SEN_L; } -void sensorInterrupt(void) { - g_sensor.interrupt(); -} - +void sensorInterrupt(void) { g_sensor.interrupt(); } -void SENSOR::interrupt(void) +void SENSOR::interrupt(void) { static char cnt = 0; @@ -47,7 +44,7 @@ void SENSOR::interrupt(void) asm("nop \n"); } sen_fr.value = analogRead(AD1); - digitalWrite(SLED_FR, LOW); //LED消灯 + digitalWrite(SLED_FR, LOW); //LED消灯 if (sen_fr.value > sen_fr.th_wall) { sen_fr.is_wall = true; } else { @@ -106,7 +103,7 @@ void SENSOR::interrupt(void) sen_l.error = 0; sen_l.is_control = false; } - battery_value = (double)analogReadMilliVolts(AD0) / 10.0 * (10.0 + 51.0); + battery_value = (double)analogReadMilliVolts(AD0) / 10.0 * (10.0 + 51.0); break; default: Serial.printf("sensor state error\n\r"); diff --git a/pico_classic_v4_STEP8_micromouse/.clang-format b/pico_classic_v4_STEP8_micromouse/.clang-format new file mode 120000 index 0000000..7cab60c --- /dev/null +++ b/pico_classic_v4_STEP8_micromouse/.clang-format @@ -0,0 +1 @@ +../.clang-format \ No newline at end of file diff --git a/pico_classic_v4_STEP8_micromouse/Flash.ino b/pico_classic_v4_STEP8_micromouse/Flash.ino index 6dce552..8bd0edd 100644 --- a/pico_classic_v4_STEP8_micromouse/Flash.ino +++ b/pico_classic_v4_STEP8_micromouse/Flash.ino @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. - -void flashInit(void) { +void flashInit(void) +{ String cmd_tmp; String file_tmp; file_tmp = "/parameters.txt"; @@ -61,9 +61,8 @@ void flashInit(void) { sensorInterruptStart(); } - -void flashBegin(void) { - +void flashBegin(void) +{ controlInterruptStop(); sensorInterruptStop(); delay(100); @@ -84,9 +83,8 @@ void flashBegin(void) { sensorInterruptStart(); } - - -void mapWrite(void) { +void mapWrite(void) +{ String file_tmp; unsigned char data_temp; file_tmp = "/map.txt"; @@ -94,7 +92,6 @@ void mapWrite(void) { controlInterruptStop(); sensorInterruptStop(); - Serial.printf("\n\r map_data write "); Serial.println(file_tmp); @@ -105,7 +102,8 @@ void mapWrite(void) { } for (int i = 0; i < 16; i++) { for (int j = 0; j < 16; j++) { - data_temp = g_map.wallDataGet(i, j, north) + (g_map.wallDataGet(i, j, east) << 2) + (g_map.wallDataGet(i, j, south) << 4) + (g_map.wallDataGet(i, j, west) << 6); + data_temp = g_map.wallDataGet(i, j, north) + (g_map.wallDataGet(i, j, east) << 2) + + (g_map.wallDataGet(i, j, south) << 4) + (g_map.wallDataGet(i, j, west) << 6); if (file.write(data_temp)) { //バイナリ書き込み } else { Serial.println("- write failed"); @@ -118,14 +116,14 @@ void mapWrite(void) { sensorInterruptStart(); } -void mapCopy(void) { +void mapCopy(void) +{ String file_tmp; unsigned char read_data; controlInterruptStop(); sensorInterruptStop(); - File file = SPIFFS.open("/map.txt", FILE_READ); if (!file || file.isDirectory()) { Serial.println("- failed to open file for reading"); @@ -149,11 +147,11 @@ void mapCopy(void) { sensorInterruptStart(); } - -void paramWrite(void) { +void paramWrite(void) +{ String cmd_tmp; String file_tmp; - char *temp_char; + char * temp_char; //タイマーによる割り込みは禁止にする必要がある。 controlInterruptStop(); @@ -197,20 +195,17 @@ void paramWrite(void) { cmd_tmp = "max_speed " + String(g_run.max_speed) + '\n'; appendFile(SPIFFS, file_tmp, cmd_tmp); - controlInterruptStart(); sensorInterruptStart(); } - - - -void paramRead(void) { +void paramRead(void) +{ String cmd_tmp; String file_tmp; int temp; - String cmds[2] = { "\0" }; // 分割された文字列を格納する配列 + String cmds[2] = {"\0"}; // 分割された文字列を格納する配列 //タイマーによる割り込みは禁止にする必要がある。 controlInterruptStop(); @@ -269,15 +264,16 @@ void paramRead(void) { break; } g_run.pulse = g_run.tire_diameter * PI / (200.0 * microstep); - cmds[0] = { "\0" }; - cmds[1] = { "\0" }; + cmds[0] = {"\0"}; + cmds[1] = {"\0"}; } file.close(); controlInterruptStart(); sensorInterruptStart(); } -int split(String data, char delimiter, String *dst) { +int split(String data, char delimiter, String * dst) +{ int index = 0; int arraySize = (sizeof(data) / sizeof((data)[0])); int datalength = data.length(); @@ -286,13 +282,14 @@ int split(String data, char delimiter, String *dst) { if (tmp == delimiter) { index++; if (index > (arraySize - 1)) return -1; - } else dst[index] += tmp; + } else + dst[index] += tmp; } return (index + 1); } - -void readFile(fs::FS &fs, String path) { +void readFile(fs::FS & fs, String path) +{ Serial.printf("Reading file: "); Serial.println(path); @@ -309,7 +306,8 @@ void readFile(fs::FS &fs, String path) { file.close(); } -void writeFile(fs::FS &fs, String path, String message) { +void writeFile(fs::FS & fs, String path, String message) +{ Serial.printf("Writing file: "); Serial.println(path); @@ -325,7 +323,8 @@ void writeFile(fs::FS &fs, String path, String message) { file.close(); } -void appendFile(fs::FS &fs, String path, String message) { +void appendFile(fs::FS & fs, String path, String message) +{ Serial.printf("Appending to file: "); Serial.println(path); diff --git a/pico_classic_v4_STEP8_micromouse/TMC5240.h b/pico_classic_v4_STEP8_micromouse/TMC5240.h index c0c1d12..2b85daf 100644 --- a/pico_classic_v4_STEP8_micromouse/TMC5240.h +++ b/pico_classic_v4_STEP8_micromouse/TMC5240.h @@ -12,7 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. - #ifndef TMC5240_H_ #define TMC5240_H_ @@ -102,19 +101,15 @@ #define TMC5240_SG4_RESULT 0x75 #define TMC5240_SG4_IND 0x76 - -class TMC5240 { +class TMC5240 +{ private: - public: unsigned int readXactual(void); void write(unsigned char add, unsigned int data_l, unsigned int data_r); void init(void); }; - extern TMC5240 g_tmc5240; - - #endif // TMC5240_H_ \ No newline at end of file diff --git a/pico_classic_v4_STEP8_micromouse/TMC5240.ino b/pico_classic_v4_STEP8_micromouse/TMC5240.ino index 0ddec08..7b33af1 100644 --- a/pico_classic_v4_STEP8_micromouse/TMC5240.ino +++ b/pico_classic_v4_STEP8_micromouse/TMC5240.ino @@ -18,7 +18,8 @@ TMC5240 g_tmc5240; portMUX_TYPE timerMux2 = portMUX_INITIALIZER_UNLOCKED; -unsigned int TMC5240::readXactual(void) { +unsigned int TMC5240::readXactual(void) +{ unsigned char spi_write_read_data[10]; unsigned int data_r, data_l; @@ -48,7 +49,9 @@ unsigned int TMC5240::readXactual(void) { digitalWrite(SPI_CS_L, HIGH); SPI.endTransaction(); - data_l = ((unsigned int)spi_write_read_data[1] << 24) | ((unsigned int)spi_write_read_data[2] << 16) | ((unsigned int)spi_write_read_data[3] << 8) | ((unsigned int)spi_write_read_data[4]); + data_l = ((unsigned int)spi_write_read_data[1] << 24) | + ((unsigned int)spi_write_read_data[2] << 16) | + ((unsigned int)spi_write_read_data[3] << 8) | ((unsigned int)spi_write_read_data[4]); for (int i = 0; i < 0xff; i++) { asm("nop \n"); @@ -79,13 +82,15 @@ unsigned int TMC5240::readXactual(void) { SPI.endTransaction(); portEXIT_CRITICAL_ISR(&timerMux2); - data_r = ((unsigned int)spi_write_read_data[1] << 24) | ((unsigned int)spi_write_read_data[2] << 16) | ((unsigned int)spi_write_read_data[3] << 8) | ((unsigned int)spi_write_read_data[4]); + data_r = ((unsigned int)spi_write_read_data[1] << 24) | + ((unsigned int)spi_write_read_data[2] << 16) | + ((unsigned int)spi_write_read_data[3] << 8) | ((unsigned int)spi_write_read_data[4]); return (unsigned int)(abs((int)data_r) + abs((int)data_l)); } -void TMC5240::write(unsigned char add, unsigned int data_l, unsigned int data_r) { - +void TMC5240::write(unsigned char add, unsigned int data_l, unsigned int data_r) +{ unsigned char spi_write_data[10]; portENTER_CRITICAL_ISR(&timerMux2); @@ -115,24 +120,29 @@ void TMC5240::write(unsigned char add, unsigned int data_l, unsigned int data_r) portEXIT_CRITICAL_ISR(&timerMux2); } -void TMC5240::init(void) { +void TMC5240::init(void) +{ SPI.begin(SPI_CLK, SPI_MISO, SPI_MOSI); - pinMode(SPI_CS_L, OUTPUT); //left + pinMode(SPI_CS_L, OUTPUT); //left pinMode(SPI_CS_R, OUTPUT); //right pinMode(SPI_CS_J, OUTPUT); digitalWrite(SPI_CS_L, HIGH); digitalWrite(SPI_CS_R, HIGH); digitalWrite(SPI_CS_J, HIGH); - g_tmc5240.write(TMC5240_DRV_CONF, 0x00000031, 0x00000031); //Current Range 2A 800V/us - g_tmc5240.write(TMC5240_IHOLD_IRUN, 0x01041C03, 0x01041C03); //IHOLDDELAY=4 IRUN=28/32 IHOLD=3/32 2A設定時ピーク1.5A、実効値1.06A - g_tmc5240.write(TMC5240_CHOPCONG, 0x04000001, 0x04000001); //MRES=0 1/16step TOFFTime=1 1以上でないとモータが動作しない - g_tmc5240.write(TMC5240_PWMCONF, 0xC40E001D, 0xC40E001D); //PWM 48.8kHz - g_tmc5240.write(TMC5240_RAMPMODE, 0x00000001, 0x00000001); //velocity mode(positive) - g_tmc5240.write(TMC5240_GCONF, 0x00000000, 0x00000010); //右のみシャフトインバース - g_tmc5240.write(TMC5240_AMAX, 0x3ffff, 0x3ffff); //加速度max 17..0 18bit - g_tmc5240.write(TMC5240_XACTUAL, 0, 0); //初期化 - g_tmc5240.write(TMC5240_VSTART, 810, 810); //16microstep min_speed=30の時の値 + g_tmc5240.write(TMC5240_DRV_CONF, 0x00000031, 0x00000031); //Current Range 2A 800V/us + g_tmc5240.write( + TMC5240_IHOLD_IRUN, 0x01041C03, + 0x01041C03); //IHOLDDELAY=4 IRUN=28/32 IHOLD=3/32 2A設定時ピーク1.5A、実効値1.06A + g_tmc5240.write( + TMC5240_CHOPCONG, 0x04000001, + 0x04000001); //MRES=0 1/16step TOFFTime=1 1以上でないとモータが動作しない + g_tmc5240.write(TMC5240_PWMCONF, 0xC40E001D, 0xC40E001D); //PWM 48.8kHz + g_tmc5240.write(TMC5240_RAMPMODE, 0x00000001, 0x00000001); //velocity mode(positive) + g_tmc5240.write(TMC5240_GCONF, 0x00000000, 0x00000010); //右のみシャフトインバース + g_tmc5240.write(TMC5240_AMAX, 0x3ffff, 0x3ffff); //加速度max 17..0 18bit + g_tmc5240.write(TMC5240_XACTUAL, 0, 0); //初期化 + g_tmc5240.write(TMC5240_VSTART, 810, 810); //16microstep min_speed=30の時の値 g_tmc5240.write(TMC5240_VMAX, 0, 0); } \ No newline at end of file diff --git a/pico_classic_v4_STEP8_micromouse/adjust.h b/pico_classic_v4_STEP8_micromouse/adjust.h index 5c8a671..0ca8fbf 100644 --- a/pico_classic_v4_STEP8_micromouse/adjust.h +++ b/pico_classic_v4_STEP8_micromouse/adjust.h @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. - #ifndef SRC_ADJUST_H_ #define SRC_ADJUST_H_ -class ADJUST { +class ADJUST +{ public: void menu(void); void mapView(void); diff --git a/pico_classic_v4_STEP8_micromouse/adjust.ino b/pico_classic_v4_STEP8_micromouse/adjust.ino index 833e6fa..719544b 100644 --- a/pico_classic_v4_STEP8_micromouse/adjust.ino +++ b/pico_classic_v4_STEP8_micromouse/adjust.ino @@ -16,7 +16,8 @@ ADJUST g_adjust; -void ADJUST::mapView(void) { +void ADJUST::mapView(void) +{ Serial.printf("\x1b[2j"); Serial.printf("\x1b[0;0H"); Serial.printf("\n\r+"); @@ -89,7 +90,8 @@ void ADJUST::mapView(void) { } } -void ADJUST::adcView(void) { +void ADJUST::adcView(void) +{ motorDisable(); while (1) { @@ -105,19 +107,22 @@ void ADJUST::adcView(void) { } } -void ADJUST::straightCheck(int section_check) { +void ADJUST::straightCheck(int section_check) +{ motorEnable(); delay(1000); g_run.accelerate(HALF_SECTION, g_run.search_speed); if (section_check > 1) { - g_run.straight(SECTION * (section_check - 1), g_run.search_speed, g_run.max_speed, g_run.search_speed); + g_run.straight( + SECTION * (section_check - 1), g_run.search_speed, g_run.max_speed, g_run.search_speed); } g_run.decelerate(HALF_SECTION, g_run.search_speed); motorDisable(); } -void ADJUST::rotationCheck(void) { +void ADJUST::rotationCheck(void) +{ motorEnable(); delay(1000); for (int i = 0; i < 8; i++) { @@ -126,7 +131,8 @@ void ADJUST::rotationCheck(void) { motorDisable(); } -void ADJUST::menu(void) { +void ADJUST::menu(void) +{ unsigned char l_mode = 1; char LED3_data; char sw; @@ -160,7 +166,8 @@ void ADJUST::menu(void) { } } -unsigned char ADJUST::modeExec(unsigned char l_mode) { +unsigned char ADJUST::modeExec(unsigned char l_mode) +{ motorDisable(); switch (l_mode) { case 1: diff --git a/pico_classic_v4_STEP8_micromouse/device.ino b/pico_classic_v4_STEP8_micromouse/device.ino index 3165e04..1e77fa6 100644 --- a/pico_classic_v4_STEP8_micromouse/device.ino +++ b/pico_classic_v4_STEP8_micromouse/device.ino @@ -12,43 +12,33 @@ // See the License for the specific language governing permissions and // limitations under the License. - -hw_timer_t* g_timer0 = NULL; -hw_timer_t* g_timer1 = NULL; - +hw_timer_t * g_timer0 = NULL; +hw_timer_t * g_timer1 = NULL; portMUX_TYPE g_timer_mux = portMUX_INITIALIZER_UNLOCKED; - -void IRAM_ATTR onTimer0(void) { +void IRAM_ATTR onTimer0(void) +{ portENTER_CRITICAL_ISR(&g_timer_mux); controlInterrupt(); portEXIT_CRITICAL_ISR(&g_timer_mux); } -void IRAM_ATTR onTimer1(void) { +void IRAM_ATTR onTimer1(void) +{ portENTER_CRITICAL_ISR(&g_timer_mux); sensorInterrupt(); portEXIT_CRITICAL_ISR(&g_timer_mux); } +void controlInterruptStart(void) { timerStart(g_timer0); } +void controlInterruptStop(void) { timerStop(g_timer0); } +void sensorInterruptStart(void) { timerStart(g_timer1); } +void sensorInterruptStop(void) { timerStop(g_timer1); } -void controlInterruptStart(void) { - timerStart(g_timer0); -} -void controlInterruptStop(void) { - timerStop(g_timer0); -} - -void sensorInterruptStart(void) { - timerStart(g_timer1); -} -void sensorInterruptStop(void) { - timerStop(g_timer1); -} - -void deviceInit(void) { +void deviceInit(void) +{ pinMode(LED0, OUTPUT); pinMode(LED1, OUTPUT); pinMode(LED2, OUTPUT); @@ -76,7 +66,6 @@ void deviceInit(void) { pinMode(MOTOR_EN, OUTPUT); digitalWrite(MOTOR_EN, LOW); - g_timer0 = timerBegin(1000000); timerAttachInterrupt(g_timer0, &onTimer0); timerAlarm(g_timer0, 1000, true, 0); @@ -91,14 +80,16 @@ void deviceInit(void) { } //LED -void ledSet(unsigned char data) { +void ledSet(unsigned char data) +{ digitalWrite(LED0, data & 0x01); digitalWrite(LED1, (data >> 1) & 0x01); digitalWrite(LED2, (data >> 2) & 0x01); digitalWrite(LED3, (data >> 3) & 0x01); } -void bledSet(char data) { +void bledSet(char data) +{ if (data & 0x01) { digitalWrite(BLED0, HIGH); } else { @@ -112,24 +103,26 @@ void bledSet(char data) { } //Buzzer -void buzzerEnable(short f) { - ledcWriteTone(BUZZER, f); -} +void buzzerEnable(short f) { ledcWriteTone(BUZZER, f); } -void buzzerDisable(void) { +void buzzerDisable(void) +{ ledcWrite(BUZZER, 1024); //duty 100% Buzzer OFF } //motor -void motorEnable(void) { +void motorEnable(void) +{ digitalWrite(MOTOR_EN, LOW); //Power ON } -void motorDisable(void) { +void motorDisable(void) +{ digitalWrite(MOTOR_EN, HIGH); //Power OFF } //SWITCH -unsigned char switchGet(void) { +unsigned char switchGet(void) +{ unsigned char ret = 0; if (digitalRead(SW_R) == LOW) { do { @@ -153,7 +146,8 @@ unsigned char switchGet(void) { } //sensor -unsigned short sensorGetR(void) { +unsigned short sensorGetR(void) +{ digitalWrite(SLED_R, HIGH); for (int i = 0; i < WAITLOOP_SLED; i++) { asm("nop \n"); @@ -162,7 +156,8 @@ unsigned short sensorGetR(void) { digitalWrite(SLED_R, LOW); return tmp; } -unsigned short sensorGetL(void) { +unsigned short sensorGetL(void) +{ digitalWrite(SLED_L, HIGH); for (int i = 0; i < WAITLOOP_SLED; i++) { asm("nop \n"); @@ -171,7 +166,8 @@ unsigned short sensorGetL(void) { digitalWrite(SLED_L, LOW); return tmp; } -unsigned short sensorGetFL(void) { +unsigned short sensorGetFL(void) +{ digitalWrite(SLED_FL, HIGH); //LED点灯 for (int i = 0; i < WAITLOOP_SLED; i++) { asm("nop \n"); @@ -180,7 +176,8 @@ unsigned short sensorGetFL(void) { digitalWrite(SLED_FL, LOW); //LED消灯 return tmp; } -unsigned short sensorGetFR(void) { +unsigned short sensorGetFR(void) +{ digitalWrite(SLED_FR, HIGH); for (int i = 0; i < WAITLOOP_SLED; i++) { asm("nop \n"); @@ -189,6 +186,7 @@ unsigned short sensorGetFR(void) { digitalWrite(SLED_FR, LOW); return tmp; } -short batteryVoltGet(void) { +short batteryVoltGet(void) +{ return (double)analogReadMilliVolts(AD0) / 10.0 * (10.0 + 51.0) * 1.01; //1.01は補正値 } diff --git a/pico_classic_v4_STEP8_micromouse/fast.h b/pico_classic_v4_STEP8_micromouse/fast.h index f064d6c..5ade9d7 100644 --- a/pico_classic_v4_STEP8_micromouse/fast.h +++ b/pico_classic_v4_STEP8_micromouse/fast.h @@ -15,10 +15,11 @@ #ifndef SRC_FAST_H_ #define SRC_FAST_H_ -class FAST { +class FAST +{ public: - void run(short gx, short gy); + private: }; diff --git a/pico_classic_v4_STEP8_micromouse/fast.ino b/pico_classic_v4_STEP8_micromouse/fast.ino index c1e44cf..eadb93b 100644 --- a/pico_classic_v4_STEP8_micromouse/fast.ino +++ b/pico_classic_v4_STEP8_micromouse/fast.ino @@ -16,7 +16,8 @@ FAST g_fast; -void FAST::run(short gx, short gy) { +void FAST::run(short gx, short gy) +{ t_global_direction glob_nextdir; int straight_count = 0; @@ -46,8 +47,9 @@ void FAST::run(short gx, short gy) { straight_count++; break; case right: - if(straight_count>0){ - g_run.straight(straight_count * SECTION, g_run.search_speed, g_run.max_speed, g_run.search_speed); + if (straight_count > 0) { + g_run.straight( + straight_count * SECTION, g_run.search_speed, g_run.max_speed, g_run.search_speed); straight_count = 0; } g_run.decelerate(HALF_SECTION, g_run.search_speed); @@ -55,8 +57,9 @@ void FAST::run(short gx, short gy) { g_run.accelerate(HALF_SECTION, g_run.search_speed); break; case left: - if(straight_count>0){ - g_run.straight(straight_count * SECTION, g_run.search_speed, g_run.max_speed, g_run.search_speed); + if (straight_count > 0) { + g_run.straight( + straight_count * SECTION, g_run.search_speed, g_run.max_speed, g_run.search_speed); straight_count = 0; } g_run.decelerate(HALF_SECTION, g_run.search_speed); @@ -70,7 +73,8 @@ void FAST::run(short gx, short gy) { g_map.axisUpdate(); } if (straight_count > 0) { - g_run.straight(straight_count * SECTION, g_run.search_speed, g_run.max_speed, g_run.search_speed); + g_run.straight( + straight_count * SECTION, g_run.search_speed, g_run.max_speed, g_run.search_speed); } g_run.decelerate(HALF_SECTION, g_run.search_speed); } diff --git a/pico_classic_v4_STEP8_micromouse/map_manager.h b/pico_classic_v4_STEP8_micromouse/map_manager.h index 4898781..a508aa7 100644 --- a/pico_classic_v4_STEP8_micromouse/map_manager.h +++ b/pico_classic_v4_STEP8_micromouse/map_manager.h @@ -31,21 +31,9 @@ typedef struct unsigned char north : 2; //北の壁情報 bit1-0 } t_wall; //壁情報を格納する構造体(ビットフィールド) -typedef enum { - front, - right, - left, - rear, - loca_dir_error -} t_local_direction; +typedef enum { front, right, left, rear, loca_dir_error } t_local_direction; -typedef enum { - north, - east, - south, - west, - glob_dir_error -} t_global_direction; +typedef enum { north, east, south, west, glob_dir_error } t_global_direction; typedef struct { @@ -54,7 +42,8 @@ typedef struct t_global_direction dir; } t_position; -class MapManager { +class MapManager +{ public: MapManager(); @@ -63,20 +52,21 @@ class MapManager { void axisUpdate(void); void nextDir(t_local_direction dir); - t_local_direction nextDirGet(char x, char y, t_global_direction *dir); - t_local_direction nextDir2Get(short x, short y, t_global_direction *dir); + t_local_direction nextDirGet(char x, char y, t_global_direction * dir); + t_local_direction nextDir2Get(short x, short y, t_global_direction * dir); void positionInit(void); void wallDataSet(unsigned char x, unsigned char y, t_global_direction dir, char data); char wallDataGet(unsigned char x, unsigned char y, t_global_direction dir); void wallSet(bool IS_SEN_FR, bool IS_SEN_R, bool IS_SEN_L); - private: unsigned short steps_map[MAZESIZE_X][MAZESIZE_Y]; //歩数マップ t_wall wall[MAZESIZE_X][MAZESIZE_Y]; //壁の情報を格納する構造体配列 - void stepMapSet(unsigned char posX, unsigned char posY, t_global_direction l_global_dir, int *little, t_global_direction *now_dir, int *priority); - t_local_direction nextGdir(t_global_direction *p_global_dir); + void stepMapSet( + unsigned char posX, unsigned char posY, t_global_direction l_global_dir, int * little, + t_global_direction * now_dir, int * priority); + t_local_direction nextGdir(t_global_direction * p_global_dir); void searchMapMake(int x, int y); void map2Make(int x, int y); int priorityGet(unsigned char x, unsigned char y, t_global_direction dir); diff --git a/pico_classic_v4_STEP8_micromouse/map_manager.ino b/pico_classic_v4_STEP8_micromouse/map_manager.ino index a034af6..dc9c459 100644 --- a/pico_classic_v4_STEP8_micromouse/map_manager.ino +++ b/pico_classic_v4_STEP8_micromouse/map_manager.ino @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. - #include "map_manager.h" MapManager g_map; //コンストラクタ -MapManager::MapManager() { +MapManager::MapManager() +{ for (int i = 0; i < MAZESIZE_X; i++) { for (int j = 0; j < MAZESIZE_Y; j++) { wall[i][j].north = wall[i][j].east = wall[i][j].south = wall[i][j].west = @@ -47,12 +47,14 @@ MapManager::MapManager() { goal_my = 0x07; } -void MapManager::positionInit(void) { +void MapManager::positionInit(void) +{ mypos.x = mypos.y = 0; mypos.dir = north; } -char MapManager::wallDataGet(unsigned char x, unsigned char y, t_global_direction l_global_dir) { +char MapManager::wallDataGet(unsigned char x, unsigned char y, t_global_direction l_global_dir) +{ switch (l_global_dir) { case north: return wall[x][y].north; @@ -70,7 +72,9 @@ char MapManager::wallDataGet(unsigned char x, unsigned char y, t_global_directio return 99; } -void MapManager::wallDataSet(unsigned char x, unsigned char y, t_global_direction l_global_dir, char data) { +void MapManager::wallDataSet( + unsigned char x, unsigned char y, t_global_direction l_global_dir, char data) +{ switch (l_global_dir) { case north: wall[x][y].north = data; @@ -87,7 +91,8 @@ void MapManager::wallDataSet(unsigned char x, unsigned char y, t_global_directio } } -void MapManager::axisUpdate(void) { +void MapManager::axisUpdate(void) +{ switch (mypos.dir) { case north: mypos.y++; @@ -104,7 +109,8 @@ void MapManager::axisUpdate(void) { } } -void MapManager::nextDir(t_local_direction l_local_dir) { +void MapManager::nextDir(t_local_direction l_local_dir) +{ if (l_local_dir == right) { switch (mypos.dir) { case north: @@ -176,7 +182,10 @@ void MapManager::wallSet(bool IS_SEN_FR, bool IS_SEN_R, bool IS_SEN_L) //壁情 } } -void MapManager::stepMapSet(unsigned char posX, unsigned char posY, t_global_direction l_global_dir, int *little, t_global_direction *now_dir, int *priority) { +void MapManager::stepMapSet( + unsigned char posX, unsigned char posY, t_global_direction l_global_dir, int * little, + t_global_direction * now_dir, int * priority) +{ int tmp_priority; tmp_priority = priorityGet(posX, posY, l_global_dir); if (steps_map[posX][posY] < *little) { @@ -191,7 +200,8 @@ void MapManager::stepMapSet(unsigned char posX, unsigned char posY, t_global_dir } } -t_local_direction MapManager::nextGdir(t_global_direction *p_global_dir) { +t_local_direction MapManager::nextGdir(t_global_direction * p_global_dir) +{ switch (*p_global_dir) { case north: switch (mypos.dir) { @@ -275,11 +285,8 @@ t_local_direction MapManager::nextGdir(t_global_direction *p_global_dir) { } } - - - - -t_local_direction MapManager::nextDirGet(char x, char y, t_global_direction *p_global_dir) { +t_local_direction MapManager::nextDirGet(char x, char y, t_global_direction * p_global_dir) +{ int little, priority; searchMapMake(x, y); @@ -313,7 +320,8 @@ t_local_direction MapManager::nextDirGet(char x, char y, t_global_direction *p_g return front; } -t_local_direction MapManager::nextDir2Get(short x, short y, t_global_direction *p_global_dir) { +t_local_direction MapManager::nextDir2Get(short x, short y, t_global_direction * p_global_dir) +{ int little, priority; map2Make(x, y); @@ -351,8 +359,8 @@ t_local_direction MapManager::nextDir2Get(short x, short y, t_global_direction * return front; } - -void MapManager::searchMapMake(int x, int y) { +void MapManager::searchMapMake(int x, int y) +{ bool change_flag; for (int i = 0; i < MAZESIZE_X; i++) { @@ -367,7 +375,8 @@ void MapManager::searchMapMake(int x, int y) { for (int i = 0; i < MAZESIZE_X; i++) { for (int j = 0; j < MAZESIZE_Y; j++) { if (steps_map[i][j] == 65535) continue; - if ((j < (MAZESIZE_Y - 1)) && (wall[i][j].north != WALL) && (steps_map[i][j + 1] == 65535)) { + if ( + (j < (MAZESIZE_Y - 1)) && (wall[i][j].north != WALL) && (steps_map[i][j + 1] == 65535)) { steps_map[i][j + 1] = steps_map[i][j] + 1; change_flag = true; } @@ -391,7 +400,8 @@ void MapManager::searchMapMake(int x, int y) { } while (change_flag == true); } -void MapManager::map2Make(int x, int y) { +void MapManager::map2Make(int x, int y) +{ bool change_flag; for (int i = 0; i < MAZESIZE_X; i++) { @@ -406,12 +416,15 @@ void MapManager::map2Make(int x, int y) { for (int i = 0; i < MAZESIZE_X; i++) { for (int j = 0; j < MAZESIZE_Y; j++) { if (steps_map[i][j] == 65535) continue; - if ((j < (MAZESIZE_Y - 1)) && (wall[i][j].north == NOWALL) && (steps_map[i][j + 1] == 65535)) { + if ( + (j < (MAZESIZE_Y - 1)) && (wall[i][j].north == NOWALL) && + (steps_map[i][j + 1] == 65535)) { steps_map[i][j + 1] = steps_map[i][j] + 1; change_flag = true; } - if ((i < (MAZESIZE_X - 1)) && (wall[i][j].east == NOWALL) && (steps_map[i + 1][j] == 65535)) { + if ( + (i < (MAZESIZE_X - 1)) && (wall[i][j].east == NOWALL) && (steps_map[i + 1][j] == 65535)) { steps_map[i + 1][j] = steps_map[i][j] + 1; change_flag = true; } @@ -430,7 +443,8 @@ void MapManager::map2Make(int x, int y) { } while (change_flag == true); } -int MapManager::priorityGet(unsigned char x, unsigned char y, t_global_direction dir) { +int MapManager::priorityGet(unsigned char x, unsigned char y, t_global_direction dir) +{ int priority; priority = 0; @@ -438,14 +452,16 @@ int MapManager::priorityGet(unsigned char x, unsigned char y, t_global_direction if (mypos.dir == dir) { priority = 2; } else if ( - ((mypos.dir == north) && (dir == south)) || ((mypos.dir == east) && (dir == west)) || ((mypos.dir == south) && (dir == north)) || ((mypos.dir == west) && (dir == east))) { + ((mypos.dir == north) && (dir == south)) || ((mypos.dir == east) && (dir == west)) || + ((mypos.dir == south) && (dir == north)) || ((mypos.dir == west) && (dir == east))) { priority = 0; } else { priority = 1; } if ( - (wall[x][y].north == _UNKNOWN) || (wall[x][y].east == _UNKNOWN) || (wall[x][y].south == _UNKNOWN) || (wall[x][y].west == _UNKNOWN)) { + (wall[x][y].north == _UNKNOWN) || (wall[x][y].east == _UNKNOWN) || + (wall[x][y].south == _UNKNOWN) || (wall[x][y].west == _UNKNOWN)) { priority += 4; } diff --git a/pico_classic_v4_STEP8_micromouse/misc.h b/pico_classic_v4_STEP8_micromouse/misc.h index 4804acf..e7e8b15 100644 --- a/pico_classic_v4_STEP8_micromouse/misc.h +++ b/pico_classic_v4_STEP8_micromouse/misc.h @@ -12,22 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. - #ifndef SRC_MISC_H_ #define SRC_MISC_H_ - -class MISC { - +class MISC +{ public: - unsigned char mode_select; + unsigned char mode_select; void modeExec(int mode); - short buttonInc(short _data, short limit, short limit_data); - short buttonDec(short _data, short limit, short limit_data); - void buttonOk(void); - void goalAppeal(void); - void errorAppeal(void); - + short buttonInc(short _data, short limit, short limit_data); + short buttonDec(short _data, short limit, short limit_data); + void buttonOk(void); + void goalAppeal(void); + void errorAppeal(void); }; extern MISC g_misc; diff --git a/pico_classic_v4_STEP8_micromouse/misc.ino b/pico_classic_v4_STEP8_micromouse/misc.ino index 7dbb72d..0ec7360 100644 --- a/pico_classic_v4_STEP8_micromouse/misc.ino +++ b/pico_classic_v4_STEP8_micromouse/misc.ino @@ -12,7 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. - #include "misc.h" MISC g_misc; @@ -78,7 +77,6 @@ void MISC::goalAppeal(void) //ゴールしたことをアピールする motorEnable(); } - void MISC::modeExec(int mode) { motorEnable(); @@ -145,4 +143,3 @@ void MISC::modeExec(int mode) } motorDisable(); } - diff --git a/pico_classic_v4_STEP8_micromouse/pico_classic_v4_STEP8_micromouse.ino b/pico_classic_v4_STEP8_micromouse/pico_classic_v4_STEP8_micromouse.ino index 8016e87..3b82f9b 100644 --- a/pico_classic_v4_STEP8_micromouse/pico_classic_v4_STEP8_micromouse.ino +++ b/pico_classic_v4_STEP8_micromouse/pico_classic_v4_STEP8_micromouse.ino @@ -12,24 +12,26 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "adjust.h" #include #include -#include "device.h" -#include "fast.h" #include +#include +#include + #include "SPIFFS.h" +#include "TMC5240.h" +#include "adjust.h" +#include "device.h" +#include "fast.h" #include "map_manager.h" #include "misc.h" #include "parameter.h" #include "run.h" #include "search.h" #include "sensor.h" -#include -#include "TMC5240.h" -#include -void setup() { +void setup() +{ // put your setup code here, to run once: deviceInit(); flashBegin(); @@ -44,7 +46,8 @@ void setup() { g_misc.mode_select = 1; } -void loop() { +void loop() +{ // put your main code here, to run repeatedly: ledSet(g_misc.mode_select); switch (switchGet()) { diff --git a/pico_classic_v4_STEP8_micromouse/run.h b/pico_classic_v4_STEP8_micromouse/run.h index 7f2f61d..fe87940 100644 --- a/pico_classic_v4_STEP8_micromouse/run.h +++ b/pico_classic_v4_STEP8_micromouse/run.h @@ -12,7 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. - #ifndef SRC_RUN_H_ #define SRC_RUN_H_ @@ -36,21 +35,21 @@ typedef enum { MOT_BACK = 2 } t_CW_CCW; -class RUN { +class RUN +{ private: - public: volatile double accel; volatile double speed; volatile double speed_target_r; - volatile double speed_target_l; + volatile double speed_target_l; volatile double upper_speed_limit; volatile double lower_speed_limit; float search_accel; short search_speed; short max_speed; float turn_accel; - t_control con_wall; + t_control con_wall; float tire_diameter; float tread_width; @@ -59,22 +58,21 @@ class RUN { RUN(); void interrupt(void); void counterClear(void); - void straight(int len, int init_speed, int max_sp, int finish_speed); + void straight(int len, int init_speed, int max_sp, int finish_speed); void accelerate(int len, int finish_speed); void oneStep(int len, int init_speed); void decelerate(int len, float init_speed); - void rotate(t_local_direction dir, int times); + void rotate(t_local_direction dir, int times); private: - int step_lr_len,step_lr; + int step_lr_len, step_lr; void dirSet(t_CW_CCW dir_left, t_CW_CCW dir_right); void speedSet(double l_speed, double r_speed); void stepGet(void); - void stay(float l_speed); - void stop(void); + void stay(float l_speed); + void stop(void); }; - extern RUN g_run; #endif /* SRC_RUN_H_ */ diff --git a/pico_classic_v4_STEP8_micromouse/run.ino b/pico_classic_v4_STEP8_micromouse/run.ino index cb26a96..55e1ddd 100644 --- a/pico_classic_v4_STEP8_micromouse/run.ino +++ b/pico_classic_v4_STEP8_micromouse/run.ino @@ -12,26 +12,23 @@ // See the License for the specific language governing permissions and // limitations under the License. - #include "TMC5240.h" #include "parameter.h" #include "run.h" RUN g_run; - - -RUN::RUN() { +RUN::RUN() +{ speed = 0.0; accel = 0.0; } //割り込み -void controlInterrupt(void) { - g_run.interrupt(); -} +void controlInterrupt(void) { g_run.interrupt(); } -void RUN::interrupt(void) { //割り込み内からコール +void RUN::interrupt(void) +{ //割り込み内からコール speed += accel; @@ -67,20 +64,22 @@ void RUN::interrupt(void) { //割り込み内からコール if (speed_target_l < MIN_SPEED) speed_target_l = MIN_SPEED; } - -void RUN::dirSet(t_CW_CCW dir_left, t_CW_CCW dir_right) { +void RUN::dirSet(t_CW_CCW dir_left, t_CW_CCW dir_right) +{ g_tmc5240.write(TMC5240_RAMPMODE, dir_left, dir_right); } -void RUN::counterClear(void) { - g_tmc5240.write(TMC5240_XACTUAL, 0, 0); -} +void RUN::counterClear(void) { g_tmc5240.write(TMC5240_XACTUAL, 0, 0); } -void RUN::speedSet(double l_speed, double r_speed) { - g_tmc5240.write(TMC5240_VMAX, (unsigned int)(l_speed / (pulse * 0.787)), (unsigned int)(r_speed / (pulse * 0.787))); +void RUN::speedSet(double l_speed, double r_speed) +{ + g_tmc5240.write( + TMC5240_VMAX, (unsigned int)(l_speed / (pulse * 0.787)), + (unsigned int)(r_speed / (pulse * 0.787))); } -void RUN::stay(float l_speed) { +void RUN::stay(float l_speed) +{ controlInterruptStop(); upper_speed_limit = lower_speed_limit = speed = (double)l_speed; accel = 0.0; @@ -89,18 +88,20 @@ void RUN::stay(float l_speed) { controlInterruptStart(); } -void RUN::stepGet(void) { +void RUN::stepGet(void) +{ step_lr = g_tmc5240.readXactual(); step_lr_len = (int)((float)step_lr / 2.0 * pulse); } -void RUN::stop(void) { +void RUN::stop(void) +{ g_tmc5240.write(TMC5240_VMAX, 0, 0); delay(300); } - -void RUN::straight(int len, int init_speed, int max_sp, int finish_speed) { +void RUN::straight(int len, int init_speed, int max_sp, int finish_speed) +{ int obj_step; controlInterruptStop(); @@ -126,7 +127,9 @@ void RUN::straight(int len, int init_speed, int max_sp, int finish_speed) { while (1) { stepGet(); speedSet(speed_target_l, speed_target_r); - if ((int)(len - step_lr_len) < (int)(((speed * speed) - (lower_speed_limit * lower_speed_limit)) / (2.0 * 1000.0 * accel))) { + if ( + (int)(len - step_lr_len) < + (int)(((speed * speed) - (lower_speed_limit * lower_speed_limit)) / (2.0 * 1000.0 * accel))) { break; } } @@ -148,7 +151,8 @@ void RUN::straight(int len, int init_speed, int max_sp, int finish_speed) { } } -void RUN::accelerate(int len, int finish_speed) { +void RUN::accelerate(int len, int finish_speed) +{ int obj_step; controlInterruptStop(); @@ -172,7 +176,8 @@ void RUN::accelerate(int len, int finish_speed) { stay(finish_speed); } -void RUN::oneStep(int len, int init_speed) { +void RUN::oneStep(int len, int init_speed) +{ int obj_step; controlInterruptStop(); @@ -195,7 +200,8 @@ void RUN::oneStep(int len, int init_speed) { stay(init_speed); } -void RUN::decelerate(int len, float init_speed) { +void RUN::decelerate(int len, float init_speed) +{ int obj_step; controlInterruptStop(); @@ -211,7 +217,9 @@ void RUN::decelerate(int len, float init_speed) { while (1) { stepGet(); speedSet(speed_target_l, speed_target_r); - if ((int)(len - step_lr_len) < (int)(((speed * speed) - (MIN_SPEED * MIN_SPEED)) / (2.0 * 1000.0 * accel))) { + if ( + (int)(len - step_lr_len) < + (int)(((speed * speed) - (MIN_SPEED * MIN_SPEED)) / (2.0 * 1000.0 * accel))) { break; } } @@ -229,8 +237,8 @@ void RUN::decelerate(int len, float init_speed) { stop(); } - -void RUN::rotate(t_local_direction dir, int times) { +void RUN::rotate(t_local_direction dir, int times) +{ int obj_step; controlInterruptStop(); @@ -257,7 +265,9 @@ void RUN::rotate(t_local_direction dir, int times) { while (1) { stepGet(); speedSet(speed_target_l, speed_target_r); - if ((int)((tread_width * PI / 4.0 * times) - step_lr_len) < (int)(((speed * speed) - (MIN_SPEED * MIN_SPEED)) / (2.0 * 1000.0 * accel))) { + if ( + (int)((tread_width * PI / 4.0 * times) - step_lr_len) < + (int)(((speed * speed) - (MIN_SPEED * MIN_SPEED)) / (2.0 * 1000.0 * accel))) { break; } } diff --git a/pico_classic_v4_STEP8_micromouse/search.h b/pico_classic_v4_STEP8_micromouse/search.h index 80d7380..02114bc 100644 --- a/pico_classic_v4_STEP8_micromouse/search.h +++ b/pico_classic_v4_STEP8_micromouse/search.h @@ -15,13 +15,13 @@ #ifndef SRC_SEARCH_H_ #define SRC_SEARCH_H_ - -class SEARCH { +class SEARCH +{ public: - void lefthand(void); - void adachi(char gx, char gy); -private: + void lefthand(void); + void adachi(char gx, char gy); +private: }; extern SEARCH g_search; #endif /* SRC_SEARCH_H_ */ \ No newline at end of file diff --git a/pico_classic_v4_STEP8_micromouse/search.ino b/pico_classic_v4_STEP8_micromouse/search.ino index ebd60b7..2e06d1e 100644 --- a/pico_classic_v4_STEP8_micromouse/search.ino +++ b/pico_classic_v4_STEP8_micromouse/search.ino @@ -16,7 +16,8 @@ SEARCH g_search; -void SEARCH::lefthand(void) { +void SEARCH::lefthand(void) +{ g_run.accelerate(HALF_SECTION, g_run.search_speed); while (1) { @@ -38,7 +39,8 @@ void SEARCH::lefthand(void) { } } -void SEARCH::adachi(char gx, char gy) { +void SEARCH::adachi(char gx, char gy) +{ t_global_direction glob_nextdir; t_local_direction temp_next_dir; @@ -76,7 +78,7 @@ void SEARCH::adachi(char gx, char gy) { case left: g_run.decelerate(HALF_SECTION, g_run.search_speed); g_run.rotate(left, 1); - g_run.accelerate(HALF_SECTION,g_run.search_speed); + g_run.accelerate(HALF_SECTION, g_run.search_speed); break; case rear: g_run.decelerate(HALF_SECTION, g_run.search_speed); diff --git a/pico_classic_v4_STEP8_micromouse/sensor.h b/pico_classic_v4_STEP8_micromouse/sensor.h index 43439ef..77c805b 100644 --- a/pico_classic_v4_STEP8_micromouse/sensor.h +++ b/pico_classic_v4_STEP8_micromouse/sensor.h @@ -29,12 +29,12 @@ typedef struct bool is_control; } t_sensor; - -class SENSOR{ +class SENSOR +{ public: - volatile t_sensor sen_r, sen_l, sen_fr, sen_fl; - volatile short battery_value; - void interrupt(void); + volatile t_sensor sen_r, sen_l, sen_fr, sen_fl; + volatile short battery_value; + void interrupt(void); }; extern SENSOR g_sensor; diff --git a/pico_classic_v4_STEP8_micromouse/sensor.ino b/pico_classic_v4_STEP8_micromouse/sensor.ino index aec3bd0..8b2e2f2 100644 --- a/pico_classic_v4_STEP8_micromouse/sensor.ino +++ b/pico_classic_v4_STEP8_micromouse/sensor.ino @@ -16,11 +16,9 @@ SENSOR g_sensor; -void sensorInterrupt(void) { - g_sensor.interrupt(); -} +void sensorInterrupt(void) { g_sensor.interrupt(); } -void SENSOR::interrupt(void) +void SENSOR::interrupt(void) { static char cnt = 0; static char bled_cnt = 0; @@ -85,7 +83,7 @@ void SENSOR::interrupt(void) } else { bledSet(2); } - if(battery_value < BATT_MIN){ + if (battery_value < BATT_MIN) { buzzerEnable(400); ledSet(0); } diff --git a/pico_classic_v4_STEP8_micromouse/webserver.ino b/pico_classic_v4_STEP8_micromouse/webserver.ino index dc27801..5b194cf 100644 --- a/pico_classic_v4_STEP8_micromouse/webserver.ino +++ b/pico_classic_v4_STEP8_micromouse/webserver.ino @@ -17,17 +17,15 @@ AsyncWebServer server(80); #ifdef AP_MODE -const char* ssid = "PICO4"; -const char* password = "12345678"; +const char * ssid = "PICO4"; +const char * password = "12345678"; #else -const char* ssid = "使用しているルータのSSID"; -const char* password = "ルータのパスワード"; +const char * ssid = "使用しているルータのSSID"; +const char * password = "ルータのパスワード"; #endif - - -void webServerSetup(void) { - +void webServerSetup(void) +{ #ifdef AP_MODE WiFi.softAP(ssid, password); IPAddress myIP = WiFi.softAPIP(); @@ -43,7 +41,7 @@ void webServerSetup(void) { Serial.println(WiFi.localIP()); #endif - server.on("/", HTTP_GET, [](AsyncWebServerRequest* request) { + server.on("/", HTTP_GET, [](AsyncWebServerRequest * request) { String html = ""; html += ""; html += ""; @@ -108,12 +106,14 @@ void webServerSetup(void) { html += ">"; - html += "SIDE ThresholdSIDE Threshold"; - html += "FRONT ThresholdFRONT Threshold

Tire Parameter

"; html += ""; - html += ""; - html += ""; html += "
TIRE_DIAMETERTIRE_DIAMETERmm
TREAD_WIDTHTREAD_WIDTHmm
"; @@ -154,7 +156,8 @@ void webServerSetup(void) { html += "

Accel Parameter

"; html += ""; - html += ""; html += "
Search accelSearch accelmm
Turn accel

Speed Parameter

"; html += ""; - html += ""; html += "
Search speedSearch speedmm
max speed"; html += "
"; - html += ""; html += "
"; html += ""; @@ -247,28 +250,27 @@ void webServerSetup(void) { request->send(200, "text/html", html); }); - server.on("/voltage", HTTP_GET, [](AsyncWebServerRequest* request) { + server.on("/voltage", HTTP_GET, [](AsyncWebServerRequest * request) { request->send(200, "text/plain", String(g_sensor.battery_value) + "mV"); }); - server.on("/left_value", HTTP_GET, [](AsyncWebServerRequest* request) { + server.on("/left_value", HTTP_GET, [](AsyncWebServerRequest * request) { request->send(200, "text/plain", String(g_sensor.sen_l.value)); }); - server.on("/right_value", HTTP_GET, [](AsyncWebServerRequest* request) { + server.on("/right_value", HTTP_GET, [](AsyncWebServerRequest * request) { request->send(200, "text/plain", String(g_sensor.sen_r.value)); }); - server.on("/left_front_value", HTTP_GET, [](AsyncWebServerRequest* request) { + server.on("/left_front_value", HTTP_GET, [](AsyncWebServerRequest * request) { request->send(200, "text/plain", String(g_sensor.sen_fl.value)); }); - server.on("/right_front_value", HTTP_GET, [](AsyncWebServerRequest* request) { + server.on("/right_front_value", HTTP_GET, [](AsyncWebServerRequest * request) { request->send(200, "text/plain", String(g_sensor.sen_fr.value)); }); - - server.on("/get", HTTP_GET, [](AsyncWebServerRequest* request) { + server.on("/get", HTTP_GET, [](AsyncWebServerRequest * request) { String inputMessage; inputMessage = request->getParam("tire_dia")->value(); @@ -309,7 +311,6 @@ void webServerSetup(void) { inputMessage = request->getParam("max_spd")->value(); g_run.max_speed = inputMessage.toInt(); - Serial.println("saved"); paramWrite(); g_run.pulse = g_run.tire_diameter * PI / (200.0 * microstep);