Shota Asao
/
RobotEx_Test01
hoaaaaaaaaaaaaa
main.cpp@0:1e31c3d92e90, 2017-12-21 (annotated)
- Committer:
- Jecours
- Date:
- Thu Dec 21 04:16:17 2017 +0000
- Revision:
- 0:1e31c3d92e90
mbed otita;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Jecours | 0:1e31c3d92e90 | 1 | #include "mbed.h" |
Jecours | 0:1e31c3d92e90 | 2 | #include "Encoder.h" |
Jecours | 0:1e31c3d92e90 | 3 | |
Jecours | 0:1e31c3d92e90 | 4 | PwmOut motor1(PC_8); |
Jecours | 0:1e31c3d92e90 | 5 | PwmOut motor2(PC_9); |
Jecours | 0:1e31c3d92e90 | 6 | DigitalOut led1(LED1); |
Jecours | 0:1e31c3d92e90 | 7 | TIM_Encoder_InitTypeDef encoder1; |
Jecours | 0:1e31c3d92e90 | 8 | TIM_Handle_TypeDef timer; |
Jecours | 0:1e31c3d92e90 | 9 | |
Jecours | 0:1e31c3d92e90 | 10 | Ticker t; |
Jecours | 0:1e31c3d92e90 | 11 | |
Jecours | 0:1e31c3d92e90 | 12 | float kp = 2.0f; |
Jecours | 0:1e31c3d92e90 | 13 | float kd = 0.0f; |
Jecours | 0:1e31c3d92e90 | 14 | float ki = 0.0f; |
Jecours | 0:1e31c3d92e90 | 15 | |
Jecours | 0:1e31c3d92e90 | 16 | void motor(int16_t a); |
Jecours | 0:1e31c3d92e90 | 17 | |
Jecours | 0:1e31c3d92e90 | 18 | uint16_t count = 0; |
Jecours | 0:1e31c3d92e90 | 19 | uint16_t countd = 0; |
Jecours | 0:1e31c3d92e90 | 20 | uint16_t integral= 0; |
Jecours | 0:1e31c3d92e90 | 21 | |
Jecours | 0:1e31c3d92e90 | 22 | uint16_t counter = 32000; |
Jecours | 0:1e31c3d92e90 | 23 | |
Jecours | 0:1e31c3d92e90 | 24 | int main() |
Jecours | 0:1e31c3d92e90 | 25 | { |
Jecours | 0:1e31c3d92e90 | 26 | EncoderInit(encoder1,timer,TIM8,65535,TIM_ENCODERMODE_TI12); |
Jecours | 0:1e31c3d92e90 | 27 | while(1) { |
Jecours | 0:1e31c3d92e90 | 28 | led1 = !led1; |
Jecours | 0:1e31c3d92e90 | 29 | wait(1); |
Jecours | 0:1e31c3d92e90 | 30 | } |
Jecours | 0:1e31c3d92e90 | 31 | } |
Jecours | 0:1e31c3d92e90 | 32 | |
Jecours | 0:1e31c3d92e90 | 33 | void calc_control(){ |
Jecours | 0:1e31c3d92e90 | 34 | countd = count; |
Jecours | 0:1e31c3d92e90 | 35 | count = __HAL_TIM_GET_COUNTER(&timer); |
Jecours | 0:1e31c3d92e90 | 36 | integral += count-countd; |
Jecours | 0:1e31c3d92e90 | 37 | int output = (counter - count)*kp + (count - countd)*kd + integral * ki; |
Jecours | 0:1e31c3d92e90 | 38 | motor(output); |
Jecours | 0:1e31c3d92e90 | 39 | } |
Jecours | 0:1e31c3d92e90 | 40 | |
Jecours | 0:1e31c3d92e90 | 41 | void motor(int16_t a) |
Jecours | 0:1e31c3d92e90 | 42 | { |
Jecours | 0:1e31c3d92e90 | 43 | float x = a/32768; |
Jecours | 0:1e31c3d92e90 | 44 | if(x>0) { |
Jecours | 0:1e31c3d92e90 | 45 | motor1.write(x); |
Jecours | 0:1e31c3d92e90 | 46 | motor2.write(0); |
Jecours | 0:1e31c3d92e90 | 47 | }else{ |
Jecours | 0:1e31c3d92e90 | 48 | x = abs(x); |
Jecours | 0:1e31c3d92e90 | 49 | motor2.write(x); |
Jecours | 0:1e31c3d92e90 | 50 | motor1.write(0); |
Jecours | 0:1e31c3d92e90 | 51 | } |
Jecours | 0:1e31c3d92e90 | 52 | } |