Shota Asao
/
RobotEx_Test01
hoaaaaaaaaaaaaa
main.cpp
- Committer:
- Jecours
- Date:
- 2017-12-21
- Revision:
- 0:1e31c3d92e90
File content as of revision 0:1e31c3d92e90:
#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); } }