Shota Asao
/
RobotEx_Test01
hoaaaaaaaaaaaaa
Diff: main.cpp
- Revision:
- 0:1e31c3d92e90
diff -r 000000000000 -r 1e31c3d92e90 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Dec 21 04:16:17 2017 +0000 @@ -0,0 +1,52 @@ +#include "mbed.h" +#include "Encoder.h" + +PwmOut motor1(PC_8); +PwmOut motor2(PC_9); +DigitalOut led1(LED1); +TIM_Encoder_InitTypeDef encoder1; +TIM_Handle_TypeDef timer; + +Ticker t; + +float kp = 2.0f; +float kd = 0.0f; +float ki = 0.0f; + +void motor(int16_t a); + +uint16_t count = 0; +uint16_t countd = 0; +uint16_t integral= 0; + +uint16_t counter = 32000; + +int main() +{ + EncoderInit(encoder1,timer,TIM8,65535,TIM_ENCODERMODE_TI12); + while(1) { + led1 = !led1; + wait(1); + } +} + +void calc_control(){ + countd = count; + count = __HAL_TIM_GET_COUNTER(&timer); + integral += count-countd; + int output = (counter - count)*kp + (count - countd)*kd + integral * ki; + motor(output); +} + +void motor(int16_t a) +{ + float x = a/32768; + if(x>0) { + motor1.write(x); + motor2.write(0); + }else{ + x = abs(x); + motor2.write(x); + motor1.write(0); + } +} \ No newline at end of file