diff --git a/uROS_STEP10_tfMsg/uROS_STEP10_tfMsg.ino b/uROS_STEP10_tfMsg/uROS_STEP10_tfMsg.ino index fa90d05..e873919 100644 --- a/uROS_STEP10_tfMsg/uROS_STEP10_tfMsg.ino +++ b/uROS_STEP10_tfMsg/uROS_STEP10_tfMsg.ino @@ -136,7 +136,7 @@ void IRAM_ATTR isrR(void) portENTER_CRITICAL_ISR(&g_timer_mux); //割り込み禁止 if (g_motor_move) { if (g_step_hz_r < 30) g_step_hz_r = 30; - timerAlarmWrite(g_timer2, 2000000 / g_step_hz_r, true); + timerAlarm(g_timer2, 2000000 / g_step_hz_r, true, 0); digitalWrite(PWM_R, HIGH); for (int i = 0; i < 100; i++) { asm("nop \n"); @@ -153,7 +153,7 @@ void IRAM_ATTR isrL(void) portENTER_CRITICAL_ISR(&g_timer_mux); //割り込み禁止 if (g_motor_move) { if (g_step_hz_l < 30) g_step_hz_l = 30; - timerAlarmWrite(g_timer3, 2000000 / g_step_hz_l, true); + timerAlarm(g_timer3, 2000000 / g_step_hz_l, true, 0); digitalWrite(PWM_L, HIGH); for (int i = 0; i < 100; i++) { asm("nop \n"); @@ -201,20 +201,20 @@ void setup() delay(2000); - g_timer0 = timerBegin(0, 80, true); //1us - timerAttachInterrupt(g_timer0, &onTimer0, true); - timerAlarmWrite(g_timer0, 1000, true); //1kHz - timerAlarmEnable(g_timer0); + g_timer0 = timerBegin(1000000); //1MHz(1us) + timerAttachInterrupt(g_timer0, &onTimer0); + timerAlarm(g_timer0, 1000, true, 0); //1000 * 1us =1000us(1kHz) + timerStart(g_timer0); - g_timer2 = timerBegin(2, 40, true); //0.5us - timerAttachInterrupt(g_timer2, &isrR, true); - timerAlarmWrite(g_timer2, 13333, true); //150Hz - timerAlarmEnable(g_timer2); + g_timer2 = timerBegin(2000000); //2MHz(0.5us) + timerAttachInterrupt(g_timer2, &isrR); + timerAlarm(g_timer2, 13333, true, 0); //13333 * 0.5us = 6666us(150Hz) + timerStart(g_timer2); - g_timer3 = timerBegin(3, 40, true); //0.5us - timerAttachInterrupt(g_timer3, &isrL, true); - timerAlarmWrite(g_timer3, 13333, true); //150Hz - timerAlarmEnable(g_timer3); + g_timer3 = timerBegin(2000000); //2MHz(0.5us) + timerAttachInterrupt(g_timer3, &isrL); + timerAlarm(g_timer3, 13333, true, 0); //13333 * 0.5us = 6666us(150Hz) + timerStart(g_timer3); g_allocator = rcl_get_default_allocator(); diff --git a/uROS_STEP11_SensorMsg/uROS_STEP11_SensorMsg.ino b/uROS_STEP11_SensorMsg/uROS_STEP11_SensorMsg.ino index 4d36b25..591333c 100644 --- a/uROS_STEP11_SensorMsg/uROS_STEP11_SensorMsg.ino +++ b/uROS_STEP11_SensorMsg/uROS_STEP11_SensorMsg.ino @@ -163,10 +163,10 @@ void setup() delay(2000); - g_timer1 = timerBegin(1, 80, true); //1us - timerAttachInterrupt(g_timer1, &onTimer1, true); - timerAlarmWrite(g_timer1, 250, true); //4kHz - timerAlarmEnable(g_timer1); + g_timer1 = timerBegin(1000000); //1MHz(1us) + timerAttachInterrupt(g_timer1, &onTimer1); + timerAlarm(g_timer1, 250, true, 0); //250 * 1us = 250us(4kHz) + timerStart(g_timer1); g_allocator = rcl_get_default_allocator(); diff --git a/uROS_STEP12_micromouse/device.h b/uROS_STEP12_micromouse/device.h index 8dddc49..b48bbbb 100644 --- a/uROS_STEP12_micromouse/device.h +++ b/uROS_STEP12_micromouse/device.h @@ -36,6 +36,4 @@ #define SW_CM 2 #define SW_RM 4 -#define BUZZER_CH 0 - #endif // DEVICE_H_ \ No newline at end of file diff --git a/uROS_STEP12_micromouse/device.ino b/uROS_STEP12_micromouse/device.ino index 61ed13b..1135770 100644 --- a/uROS_STEP12_micromouse/device.ino +++ b/uROS_STEP12_micromouse/device.ino @@ -60,7 +60,7 @@ void IRAM_ATTR isrR(void) portENTER_CRITICAL_ISR(&g_timer_mux); if (g_motor_move) { if (g_step_hz_r < 30) g_step_hz_r = 30; - timerAlarmWrite(g_timer2, 2000000 / g_step_hz_r, true); + timerAlarm(g_timer2, 2000000 / g_step_hz_r, true, 0); digitalWrite(PWM_R, HIGH); for (int i = 0; i < 100; i++) { asm("nop \n"); @@ -76,7 +76,7 @@ void IRAM_ATTR isrL(void) portENTER_CRITICAL_ISR(&g_timer_mux); if (g_motor_move) { if (g_step_hz_l < 30) g_step_hz_l = 30; - timerAlarmWrite(g_timer3, 2000000 / g_step_hz_l, true); + timerAlarm(g_timer3, 2000000 / g_step_hz_l, true, 0); digitalWrite(PWM_L, HIGH); for (int i = 0; i < 100; i++) { asm("nop \n"); @@ -87,21 +87,21 @@ void IRAM_ATTR isrL(void) portEXIT_CRITICAL_ISR(&g_timer_mux); } -void controlInterruptStart(void) { timerAlarmEnable(g_timer0); } -void controlInterruptStop(void) { timerAlarmDisable(g_timer0); } +void controlInterruptStart(void) { timerStart(g_timer0); } +void controlInterruptStop(void) { timerStop(g_timer0); } -void sensorInterruptStart(void) { timerAlarmEnable(g_timer1); } -void sensorInterruptStop(void) { timerAlarmDisable(g_timer1); } +void sensorInterruptStart(void) { timerStart(g_timer1); } +void sensorInterruptStop(void) { timerStop(g_timer1); } void PWMInterruptStart(void) { - timerAlarmEnable(g_timer2); - timerAlarmEnable(g_timer3); + timerStart(g_timer2); + timerStart(g_timer3); } void PWMInterruptStop(void) { - timerAlarmDisable(g_timer2); - timerAlarmDisable(g_timer3); + timerStop(g_timer2); + timerStop(g_timer3); } void initDevice(void) @@ -118,9 +118,8 @@ void initDevice(void) pinMode(SW_C, INPUT); pinMode(SW_R, INPUT); - ledcSetup(0, 440, 10); - ledcAttachPin(BUZZER, 0); - ledcWrite(0, 1024); + ledcAttach(BUZZER, 440, 10); + ledcWrite(BUZZER, 1024); pinMode(SLED_FR, OUTPUT); pinMode(SLED_FL, OUTPUT); @@ -149,25 +148,25 @@ void initDevice(void) } } - g_timer0 = timerBegin(0, 80, true); - timerAttachInterrupt(g_timer0, &onTimer0, false); - timerAlarmWrite(g_timer0, 1000, true); - timerAlarmEnable(g_timer0); + g_timer0 = timerBegin(1000000); + timerAttachInterrupt(g_timer0, &onTimer0); + timerAlarm(g_timer0, 1000, true, 0); + timerStart(g_timer0); - g_timer1 = timerBegin(1, 80, true); - timerAttachInterrupt(g_timer1, &onTimer1, true); - timerAlarmWrite(g_timer1, 500, true); - timerAlarmEnable(g_timer1); + g_timer1 = timerBegin(1000000); + timerAttachInterrupt(g_timer1, &onTimer1); + timerAlarm(g_timer1, 500, true, 0); + timerStart(g_timer1); - g_timer2 = timerBegin(2, 40, true); - timerAttachInterrupt(g_timer2, &isrR, false); - timerAlarmWrite(g_timer2, 13333, true); - timerAlarmEnable(g_timer2); + g_timer2 = timerBegin(2000000); + timerAttachInterrupt(g_timer2, &isrR); + timerAlarm(g_timer2, 13333, true, 0); + timerStart(g_timer2); - g_timer3 = timerBegin(3, 40, true); - timerAttachInterrupt(g_timer3, &isrL, false); - timerAlarmWrite(g_timer3, 13333, true); - timerAlarmEnable(g_timer3); + g_timer3 = timerBegin(2000000); + timerAttachInterrupt(g_timer3, &isrL); + timerAlarm(g_timer3, 13333, true, 0); + timerStart(g_timer3); Serial.begin(115200); @@ -199,10 +198,10 @@ void setBLED(char data) } //Buzzer -void enableBuzzer(short f) { ledcWriteTone(BUZZER_CH, f); } +void enableBuzzer(short f) { ledcWriteTone(BUZZER, f); } void disableBuzzer(void) { - ledcWrite(BUZZER_CH, 1024); //duty 100% Buzzer OFF + ledcWrite(BUZZER, 1024); //duty 100% Buzzer OFF } //motor @@ -266,8 +265,7 @@ unsigned short getSensorR(void) for (int i = 0; i < WAITLOOP_SLED; i++) { asm("nop \n"); } - // unsigned short tmp = analogRead(AD3); - unsigned short tmp = adc1_get_raw(ADC1_CHANNEL_5); + unsigned short tmp = analogRead(AD3); digitalWrite(SLED_R, LOW); return tmp; } @@ -277,8 +275,7 @@ unsigned short getSensorL(void) for (int i = 0; i < WAITLOOP_SLED; i++) { asm("nop \n"); } - // unsigned short tmp = analogRead(AD4); - unsigned short tmp = adc1_get_raw(ADC1_CHANNEL_6); + unsigned short tmp = analogRead(AD4); digitalWrite(SLED_L, LOW); return tmp; } @@ -288,8 +285,7 @@ unsigned short getSensorFL(void) for (int i = 0; i < WAITLOOP_SLED; i++) { asm("nop \n"); } - // unsigned short tmp = analogRead(AD2); - unsigned short tmp = adc1_get_raw(ADC1_CHANNEL_4); + unsigned short tmp = analogRead(AD2); digitalWrite(SLED_FL, LOW); //LED消灯 return tmp; } @@ -299,8 +295,7 @@ unsigned short getSensorFR(void) for (int i = 0; i < WAITLOOP_SLED; i++) { asm("nop \n"); } - // unsigned short tmp = analogRead(AD1); - unsigned short tmp = adc1_get_raw(ADC1_CHANNEL_3); + unsigned short tmp = analogRead(AD1); digitalWrite(SLED_FR, LOW); return tmp; } diff --git a/uROS_STEP3_Buzzer/uROS_STEP3_Buzzer.ino b/uROS_STEP3_Buzzer/uROS_STEP3_Buzzer.ino index 048f15f..f51a13f 100644 --- a/uROS_STEP3_Buzzer/uROS_STEP3_Buzzer.ino +++ b/uROS_STEP3_Buzzer/uROS_STEP3_Buzzer.ino @@ -60,22 +60,22 @@ void execByMode(char mode) { switch (mode) { case 1: - ledcWriteTone(0, FREQ_C); + ledcWriteTone(BUZZER, FREQ_C); delay(1000); - ledcWrite(0, 1024); + ledcWrite(BUZZER, 1024); break; case 2: - ledcWriteTone(0, FREQ_D); + ledcWriteTone(BUZZER, FREQ_D); delay(1000); - ledcWrite(0, 1024); + ledcWrite(BUZZER, 1024); break; case 3: - ledcWriteTone(0, FREQ_E); + ledcWriteTone(BUZZER, FREQ_E); delay(1000); - ledcWrite(0, 1024); + ledcWrite(BUZZER, 1024); break; default: - ledcWrite(0, 1024); + ledcWrite(BUZZER, 1024); break; } } @@ -92,9 +92,8 @@ void setup() pinMode(SW_C, INPUT); pinMode(SW_R, INPUT); - ledcSetup(0, 440, 10); - ledcAttachPin(BUZZER, 0); - ledcWrite(0, 1024); + ledcAttach(BUZZER, 440, 10); + ledcWrite(BUZZER, 1024); g_mode = 1; setLED(g_mode); @@ -111,9 +110,9 @@ void loop() if (g_mode > 15) { g_mode = 15; } else { - ledcWriteTone(0, INC_FREQ); + ledcWriteTone(BUZZER, INC_FREQ); delay(30); - ledcWrite(0, 1024); + ledcWrite(BUZZER, 1024); } setLED(g_mode); } @@ -122,18 +121,18 @@ void loop() if (g_mode < 1) { g_mode = 1; } else { - ledcWriteTone(0, DEC_FREQ); + ledcWriteTone(BUZZER, DEC_FREQ); delay(30); - ledcWrite(0, 1024); + ledcWrite(BUZZER, 1024); } setLED(g_mode); } if (digitalRead(SW_C) == 0) { - ledcWriteTone(0, INC_FREQ); + ledcWriteTone(BUZZER, INC_FREQ); delay(80); - ledcWriteTone(0, DEC_FREQ); + ledcWriteTone(BUZZER, DEC_FREQ); delay(80); - ledcWrite(0, 1024); + ledcWrite(BUZZER, 1024); delay(300); execByMode(g_mode); } diff --git a/uROS_STEP4_Sensor/uROS_STEP4_Sensor.ino b/uROS_STEP4_Sensor/uROS_STEP4_Sensor.ino index a5454e5..eb1dc96 100644 --- a/uROS_STEP4_Sensor/uROS_STEP4_Sensor.ino +++ b/uROS_STEP4_Sensor/uROS_STEP4_Sensor.ino @@ -94,10 +94,10 @@ void setup() Serial.begin(115200); - g_timer1 = timerBegin(1, 80, true); //1us - timerAttachInterrupt(g_timer1, &onTimer1, true); - timerAlarmWrite(g_timer1, 250, true); //4kHz - timerAlarmEnable(g_timer1); + g_timer1 = timerBegin(1000000); //1MHz(1us) + timerAttachInterrupt(g_timer1, &onTimer1); + timerAlarm(g_timer1, 250, true, 0); //250 * 1us =250us(4kHz) + timerStart(g_timer1); } void loop() diff --git a/uROS_STEP5_Straight/uROS_STEP5_Straight.ino b/uROS_STEP5_Straight/uROS_STEP5_Straight.ino index 6c77f5a..f5fa6c0 100644 --- a/uROS_STEP5_Straight/uROS_STEP5_Straight.ino +++ b/uROS_STEP5_Straight/uROS_STEP5_Straight.ino @@ -63,7 +63,7 @@ void IRAM_ATTR isrR(void) { portENTER_CRITICAL_ISR(&g_timer_mux); //割り込み禁止 if (g_motor_move) { - timerAlarmWrite(g_timer2, 2000000 / g_step_hz_r, true); + timerAlarm(g_timer2, 2000000 / g_step_hz_r, true, 0); digitalWrite(PWM_R, HIGH); for (int i = 0; i < 100; i++) { asm("nop \n"); @@ -79,7 +79,7 @@ void IRAM_ATTR isrL(void) { portENTER_CRITICAL_ISR(&g_timer_mux); //割り込み禁止 if (g_motor_move) { - timerAlarmWrite(g_timer3, 2000000 / g_step_hz_l, true); + timerAlarm(g_timer3, 2000000 / g_step_hz_l, true, 0); digitalWrite(PWM_L, HIGH); for (int i = 0; i < 100; i++) { asm("nop \n"); @@ -115,20 +115,20 @@ void setup() digitalWrite(PWM_R, LOW); digitalWrite(PWM_L, LOW); - g_timer0 = timerBegin(0, 80, true); //1us - timerAttachInterrupt(g_timer0, &onTimer0, true); - timerAlarmWrite(g_timer0, 1000, true); //1kHz - timerAlarmEnable(g_timer0); + g_timer0 = timerBegin(1000000); //1MHz(1us) + timerAttachInterrupt(g_timer0, &onTimer0); + timerAlarm(g_timer0, 1000, true, 0); //1000 * 1us =1000us(1kHz) + timerStart(g_timer0); - g_timer2 = timerBegin(2, 40, true); //0.5us - timerAttachInterrupt(g_timer2, isrR, true); - timerAlarmWrite(g_timer2, 13333, true); //150Hz - timerAlarmEnable(g_timer2); + g_timer2 = timerBegin(2000000); //2MHz(0.5us) + timerAttachInterrupt(g_timer2, isrR); + timerAlarm(g_timer2, 13333, true, 0); //13333 * 0.5us = 6666us(150Hz) + timerStart(g_timer2); - g_timer3 = timerBegin(3, 40, true); //0.5us - timerAttachInterrupt(g_timer3, &isrL, true); - timerAlarmWrite(g_timer3, 13333, true); //150Hz - timerAlarmEnable(g_timer3); + g_timer3 = timerBegin(2000000); //2MHz(0.5us) + timerAttachInterrupt(g_timer3, &isrL); + timerAlarm(g_timer3, 13333, true, 0); //13333 * 0.5us = 6666us(150Hz) + timerStart(g_timer3); } void loop() diff --git a/uROS_STEP6_rotate/uROS_STEP6_rotate.ino b/uROS_STEP6_rotate/uROS_STEP6_rotate.ino index a4d6ff6..2797f4f 100644 --- a/uROS_STEP6_rotate/uROS_STEP6_rotate.ino +++ b/uROS_STEP6_rotate/uROS_STEP6_rotate.ino @@ -72,7 +72,7 @@ void IRAM_ATTR isrR(void) { portENTER_CRITICAL_ISR(&g_timer_mux); //割り込み禁止 if (g_motor_move) { - timerAlarmWrite(g_timer2, 2000000 / g_step_hz_r, true); + timerAlarm(g_timer2, 2000000 / g_step_hz_r, true, 0); digitalWrite(PWM_R, HIGH); for (int i = 0; i < 100; i++) { asm("nop \n"); @@ -88,7 +88,7 @@ void IRAM_ATTR isrL(void) { portENTER_CRITICAL_ISR(&g_timer_mux); //割り込み禁止 if (g_motor_move) { - timerAlarmWrite(g_timer3, 2000000 / g_step_hz_l, true); + timerAlarm(g_timer3, 2000000 / g_step_hz_l, true, 0); digitalWrite(PWM_L, HIGH); for (int i = 0; i < 100; i++) { asm("nop \n"); @@ -124,20 +124,20 @@ void setup() digitalWrite(PWM_R, LOW); digitalWrite(PWM_L, LOW); - g_timer0 = timerBegin(0, 80, true); //1us - timerAttachInterrupt(g_timer0, &onTimer0, true); - timerAlarmWrite(g_timer0, 1000, true); //1kHz - timerAlarmEnable(g_timer0); + g_timer0 = timerBegin(1000000); //1MHz(1us) + timerAttachInterrupt(g_timer0, &onTimer0); + timerAlarm(g_timer0, 1000, true, 0); //1000 * 1us = 1000us(1kHz) + timerStart(g_timer0); - g_timer2 = timerBegin(2, 40, true); //0.5us - timerAttachInterrupt(g_timer2, &isrR, true); - timerAlarmWrite(g_timer2, 13333, true); //150Hz - timerAlarmEnable(g_timer2); + g_timer2 = timerBegin(2000000); //2MHz(0.5us) + timerAttachInterrupt(g_timer2, &isrR); + timerAlarm(g_timer2, 13333, true, 0); //13333 * 0.5us = 6666us(150Hz) + timerStart(g_timer2); - g_timer3 = timerBegin(3, 40, true); //0.5us - timerAttachInterrupt(g_timer3, &isrL, true); - timerAlarmWrite(g_timer3, 13333, true); //150Hz - timerAlarmEnable(g_timer3); + g_timer3 = timerBegin(2000000); //2MHz(0.5us) + timerAttachInterrupt(g_timer3, &isrL); + timerAlarm(g_timer3, 13333, true, 0); //13333 * 0.5us = 6666us(150Hz) + timerStart(g_timer3); } void loop() diff --git a/uROS_STEP7_P_control/uROS_STEP7_P_control.ino b/uROS_STEP7_P_control/uROS_STEP7_P_control.ino index cfeefa0..a64c2c8 100644 --- a/uROS_STEP7_P_control/uROS_STEP7_P_control.ino +++ b/uROS_STEP7_P_control/uROS_STEP7_P_control.ino @@ -127,7 +127,7 @@ void IRAM_ATTR isrR(void) { portENTER_CRITICAL_ISR(&g_timer_mux); //割り込み禁止 if (g_motor_move) { - timerAlarmWrite(g_timer2, 2000000 / g_step_hz_r, true); + timerAlarm(g_timer2, 2000000 / g_step_hz_r, true, 0); digitalWrite(PWM_R, HIGH); for (int i = 0; i < 100; i++) { asm("nop \n"); @@ -143,7 +143,7 @@ void IRAM_ATTR isrL(void) { portENTER_CRITICAL_ISR(&g_timer_mux); //割り込み禁止 if (g_motor_move) { - timerAlarmWrite(g_timer3, 2000000 / g_step_hz_l, true); + timerAlarm(g_timer3, 2000000 / g_step_hz_l, true, 0); digitalWrite(PWM_L, HIGH); for (int i = 0; i < 100; i++) { asm("nop \n"); @@ -190,25 +190,25 @@ void setup() Serial.begin(115200); - g_timer0 = timerBegin(0, 80, true); //1us - timerAttachInterrupt(g_timer0, &onTimer0, true); - timerAlarmWrite(g_timer0, 1000, true); //1kHz - timerAlarmEnable(g_timer0); - - g_timer1 = timerBegin(1, 80, true); - timerAttachInterrupt(g_timer1, &onTimer1, false); - timerAlarmWrite(g_timer1, 250, true); - timerAlarmEnable(g_timer1); - - g_timer2 = timerBegin(2, 40, true); //0.5us - timerAttachInterrupt(g_timer2, &isrR, true); - timerAlarmWrite(g_timer2, 13333, true); //150Hz - timerAlarmEnable(g_timer2); - - g_timer3 = timerBegin(3, 40, true); //0.5us - timerAttachInterrupt(g_timer3, &isrL, true); - timerAlarmWrite(g_timer3, 13333, true); //150Hz - timerAlarmEnable(g_timer3); + g_timer0 = timerBegin(1000000); //1MHz(1us) + timerAttachInterrupt(g_timer0, &onTimer0); + timerAlarm(g_timer0, 1000, true, 0); //1000 * 1us =1000us(1kHz) + timerStart(g_timer0); + + g_timer1 = timerBegin(1000000); + timerAttachInterrupt(g_timer1, &onTimer1); + timerAlarm(g_timer1, 250, true, 0); + timerStart(g_timer1); + + g_timer2 = timerBegin(2000000); //2MHz(0.5us) + timerAttachInterrupt(g_timer2, &isrR); + timerAlarm(g_timer2, 13333, true, 0); //13333 * 0.5us = 6666us(150Hz) + timerStart(g_timer2); + + g_timer3 = timerBegin(2000000); //2MHz(0.5us) + timerAttachInterrupt(g_timer3, &isrL); + timerAlarm(g_timer3, 13333, true, 0); //13333 * 0.5us = 6666us(150Hz) + timerStart(g_timer3); g_sen_r.ref = REF_SEN_R; g_sen_l.ref = REF_SEN_L; diff --git a/uROS_STEP8_micromouse/device.h b/uROS_STEP8_micromouse/device.h index 8dddc49..b48bbbb 100644 --- a/uROS_STEP8_micromouse/device.h +++ b/uROS_STEP8_micromouse/device.h @@ -36,6 +36,4 @@ #define SW_CM 2 #define SW_RM 4 -#define BUZZER_CH 0 - #endif // DEVICE_H_ \ No newline at end of file diff --git a/uROS_STEP8_micromouse/device.ino b/uROS_STEP8_micromouse/device.ino index 821e6db..600c229 100644 --- a/uROS_STEP8_micromouse/device.ino +++ b/uROS_STEP8_micromouse/device.ino @@ -53,7 +53,7 @@ void IRAM_ATTR isrR(void) portENTER_CRITICAL_ISR(&g_timer_mux); if (g_motor_move) { if (g_step_hz_r < 30) g_step_hz_r = 30; - timerAlarmWrite(g_timer2, 2000000 / g_step_hz_r, true); + timerAlarm(g_timer2, 2000000 / g_step_hz_r, true, 0); digitalWrite(PWM_R, HIGH); for (int i = 0; i < 100; i++) { asm("nop \n"); @@ -69,7 +69,7 @@ void IRAM_ATTR isrL(void) portENTER_CRITICAL_ISR(&g_timer_mux); if (g_motor_move) { if (g_step_hz_l < 30) g_step_hz_l = 30; - timerAlarmWrite(g_timer3, 2000000 / g_step_hz_l, true); + timerAlarm(g_timer3, 2000000 / g_step_hz_l, true, 0); digitalWrite(PWM_L, HIGH); for (int i = 0; i < 100; i++) { asm("nop \n"); @@ -80,21 +80,21 @@ void IRAM_ATTR isrL(void) portEXIT_CRITICAL_ISR(&g_timer_mux); } -void controlInterruptStart(void) { timerAlarmEnable(g_timer0); } -void controlInterruptStop(void) { timerAlarmDisable(g_timer0); } +void controlInterruptStart(void) { timerStart(g_timer0); } +void controlInterruptStop(void) { timerStop(g_timer0); } -void sensorInterruptStart(void) { timerAlarmEnable(g_timer1); } -void sensorInterruptStop(void) { timerAlarmDisable(g_timer1); } +void sensorInterruptStart(void) { timerStart(g_timer1); } +void sensorInterruptStop(void) { timerStop(g_timer1); } void PWMInterruptStart(void) { - timerAlarmEnable(g_timer2); - timerAlarmEnable(g_timer3); + timerStart(g_timer2); + timerStart(g_timer3); } void PWMInterruptStop(void) { - timerAlarmDisable(g_timer2); - timerAlarmDisable(g_timer3); + timerStop(g_timer2); + timerStop(g_timer3); } void initAll(void) @@ -111,9 +111,8 @@ void initAll(void) pinMode(SW_C, INPUT); pinMode(SW_R, INPUT); - ledcSetup(0, 440, 10); - ledcAttachPin(BUZZER, 0); - ledcWrite(0, 1024); + ledcAttach(BUZZER, 440, 10); + ledcWrite(BUZZER, 1024); pinMode(SLED_FR, OUTPUT); pinMode(SLED_FL, OUTPUT); @@ -142,25 +141,25 @@ void initAll(void) } } - g_timer0 = timerBegin(0, 80, true); - timerAttachInterrupt(g_timer0, &onTimer0, false); - timerAlarmWrite(g_timer0, 1000, true); - timerAlarmEnable(g_timer0); + g_timer0 = timerBegin(1000000); + timerAttachInterrupt(g_timer0, &onTimer0); + timerAlarm(g_timer0, 1000, true, 0); + timerStart(g_timer0); - g_timer1 = timerBegin(1, 80, true); - timerAttachInterrupt(g_timer1, &onTimer1, false); - timerAlarmWrite(g_timer1, 250, true); - timerAlarmEnable(g_timer1); + g_timer1 = timerBegin(1000000); + timerAttachInterrupt(g_timer1, &onTimer1); + timerAlarm(g_timer1, 250, true, 0); + timerStart(g_timer1); - g_timer2 = timerBegin(2, 40, true); - timerAttachInterrupt(g_timer2, &isrR, false); - timerAlarmWrite(g_timer2, 13333, true); - timerAlarmEnable(g_timer2); + g_timer2 = timerBegin(2000000); + timerAttachInterrupt(g_timer2, &isrR); + timerAlarm(g_timer2, 13333, true, 0); + timerStart(g_timer2); - g_timer3 = timerBegin(3, 40, true); - timerAttachInterrupt(g_timer3, &isrL, false); - timerAlarmWrite(g_timer3, 13333, true); - timerAlarmEnable(g_timer3); + g_timer3 = timerBegin(2000000); + timerAttachInterrupt(g_timer3, &isrL); + timerAlarm(g_timer3, 13333, true, 0); + timerStart(g_timer3); Serial.begin(115200); @@ -212,10 +211,10 @@ void setBLED(char data) } //Buzzer -void enableBuzzer(short f) { ledcWriteTone(BUZZER_CH, f); } +void enableBuzzer(short f) { ledcWriteTone(BUZZER, f); } void disableBuzzer(void) { - ledcWrite(BUZZER_CH, 1024); //duty 100% Buzzer OFF + ledcWrite(BUZZER, 1024); //duty 100% Buzzer OFF } //motor diff --git a/uROS_STEP9_twistMsg/uROS_STEP9_twistMsg.ino b/uROS_STEP9_twistMsg/uROS_STEP9_twistMsg.ino index 8cf5d5e..c28a188 100644 --- a/uROS_STEP9_twistMsg/uROS_STEP9_twistMsg.ino +++ b/uROS_STEP9_twistMsg/uROS_STEP9_twistMsg.ino @@ -109,7 +109,7 @@ void IRAM_ATTR isrR(void) portENTER_CRITICAL_ISR(&g_timer_mux); //割り込み禁止 if (g_motor_move) { if (g_step_hz_r < 30) g_step_hz_r = 30; - timerAlarmWrite(g_timer2, 2000000 / g_step_hz_r, true); + timerAlarm(g_timer2, 2000000 / g_step_hz_r, true, 0); digitalWrite(PWM_R, HIGH); for (int i = 0; i < 100; i++) { asm("nop \n"); @@ -126,7 +126,7 @@ void IRAM_ATTR isrL(void) portENTER_CRITICAL_ISR(&g_timer_mux); //割り込み禁止 if (g_motor_move) { if (g_step_hz_l < 30) g_step_hz_l = 30; - timerAlarmWrite(g_timer3, 2000000 / g_step_hz_l, true); + timerAlarm(g_timer3, 2000000 / g_step_hz_l, true, 0); digitalWrite(PWM_L, HIGH); for (int i = 0; i < 100; i++) { asm("nop \n"); @@ -174,20 +174,20 @@ void setup() delay(2000); - g_timer0 = timerBegin(0, 80, true); //1us - timerAttachInterrupt(g_timer0, &onTimer0, true); - timerAlarmWrite(g_timer0, 1000, true); //1kHz - timerAlarmEnable(g_timer0); + g_timer0 = timerBegin(1000000); //1MHz(1us) + timerAttachInterrupt(g_timer0, &onTimer0); + timerAlarm(g_timer0, 1000, true, 0); //1000 * 1us = 1000us(1kHz) + timerStart(g_timer0); - g_timer2 = timerBegin(2, 40, true); //0.5us - timerAttachInterrupt(g_timer2, &isrR, true); - timerAlarmWrite(g_timer2, 13333, true); //150Hz - timerAlarmEnable(g_timer2); + g_timer2 = timerBegin(2000000); //2MHz(0.5us) + timerAttachInterrupt(g_timer2, &isrR); + timerAlarm(g_timer2, 13333, true, 0); //13333 * 0.5us = 6666us(150Hz) + timerStart(g_timer2); - g_timer3 = timerBegin(3, 40, true); //0.5us - timerAttachInterrupt(g_timer3, &isrL, true); - timerAlarmWrite(g_timer3, 13333, true); //150Hz - timerAlarmEnable(g_timer3); + g_timer3 = timerBegin(2000000); //2MHz(0.5us) + timerAttachInterrupt(g_timer3, &isrL); + timerAlarm(g_timer3, 13333, true, 0); //13333 * 0.5us = 6666us(150Hz) + timerStart(g_timer3); g_allocator = rcl_get_default_allocator();