Diff: main.cpp

Revision:

0:1e31c3d92e90

--- /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