motorTest
Dependencies: mbed
main.cpp
- Committer:
- eric80739
- Date:
- 2018-04-12
- Revision:
- 0:6b47dfc0d6aa
File content as of revision 0:6b47dfc0d6aa:
#include "mbed.h" #define SERVO_PWM_PERIOD 20 //ms #define MOTOR_PWM_PERIOD 50 //us #define SERVO_INITIAL_DUTYCYCLE 0.05 #define MOTOR_INITIAL_DUTYCYCLE 0.5 #define PI 3.14159265 DigitalOut led_1(PC_0); DigitalOut led_2(PC_1); Ticker servoTimer; Ticker motorTimer; PwmOut servo_cmd(PA_0); PwmOut motor_cmd_1(D7); PwmOut motor_cmd_1N(D11); void motorTimer_interrupt(); void init_servo(); void init_led(); void servoTimer_interrupt(); void init_timer(); int main() { init_led(); init_servo(); init_timer(); while(1) { ; } } void init_led() { led_1 = 0; led_2 = 0; } void init_servo() { servo_cmd.period_ms(SERVO_PWM_PERIOD); servo_cmd.write(SERVO_INITIAL_DUTYCYCLE); motor_cmd_1.period_us(MOTOR_PWM_PERIOD); motor_cmd_1.write(MOTOR_INITIAL_DUTYCYCLE); TIM1->CCER |= 0x4; } void init_timer(void) { servoTimer.attach(&servoTimer_interrupt, 1.0); //1s motorTimer.attach(&motorTimer_interrupt, 3.0); //1s } void servoTimer_interrupt() { static double dutyCycle = SERVO_INITIAL_DUTYCYCLE; static int n=0; //在這裡用7是為了讓一開始可以矯正 if((n!=0) && (n<7)) { dutyCycle += 0.007333; } else if ((7<=n) && (n<13)) { dutyCycle -= 0.007333; } servo_cmd.write(dutyCycle); n++; if (n>=13) { n = 1; } } void motorTimer_interrupt() { static int n = 0; static float dutyCycle = 0.5; n++; if (n >= 6) n--; dutyCycle = MOTOR_INITIAL_DUTYCYCLE + n*0.1; motor_cmd_1.write(dutyCycle); TIM1->CCER |= 0x4; }