Diff: main.cpp

Revision:

0:25526d7f05c5

Child:

1:dfc7b884cca0

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Dec 23 02:45:25 2019 +0000
@@ -0,0 +1,140 @@
+#include "mbed.h"
+#include "math.h"
+#include "encoder.h"//#include "Encoder.h"//encoder
+#include "pid.h"//position_and_velocity_PID
+#include "core_cm3.h" //NVIC_Setting
+#include "misc.h" //NVIC_Setting
+
+#define t_pid 0.5
+#define Kp 0.00
+#define Ki 0.0005
+#define Kd 0.0
+#define pc_tx       USBTX
+#define pc_rx       USBRX
+#define pc_baud     460800
+
+Encoder encoder_vibrate(D11, D12);
+Encoder encoder_outaxis(D13, D14);
+DigitalOut m_dir(D2);
+PwmOut m_duty(D3);
+Serial pc(USBTX,USBRX,460800);
+SpeedPid speedPid;
+Ticker calc_pid;
+DigitalIn button(USER_BUTTON);
+
+void debug();
+void duty();
+void accelerate();
+int num_pidticker=0;
+float rpm_target = 0;//inital_value(increasable later)
+float sec=0;
+bool renew = false;
+int tim_renew = 0;
+bool button_state;
+
+/*************
+setup_function
+*************/
+void setup() {
+	//NVIC Initialize
+	NVIC_InitTypeDef NVIC_InitStructure;
+	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
+
+	//// Enable the GPIO Interrupt for Encorder 
+	NVIC_InitStructure.NVIC_IRQChannel = EXTI9_5_IRQn;
+	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
+	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+	NVIC_Init(&NVIC_InitStructure);
+
+	//// Enable the TIM4 Interrupt
+	NVIC_InitStructure.NVIC_IRQChannel = TIM4_IRQn;
+	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
+	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+	NVIC_Init(&NVIC_InitStructure);
+	
+	//// Change the Systick priority 
+	NVIC_InitStructure.NVIC_IRQChannel = SysTick_IRQn;
+	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
+	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
+	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+	NVIC_Init(&NVIC_InitStructure);
+
+
+	//// Enable the VCP Interrupt
+	NVIC_InitStructure.NVIC_IRQChannel = USB_HP_CAN1_TX_IRQn;
+	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;
+	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+	NVIC_Init(&NVIC_InitStructure);
+
+	NVIC_InitStructure.NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn;
+	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;
+	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
+	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+	NVIC_Init(&NVIC_InitStructure);
+
+	//// Enable the TIM2 Interrupt
+	NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQn;
+	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 3;
+	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+	NVIC_Init(&NVIC_InitStructure);
+
+
+    pc.printf("%4.2f Hz (%6.2f rpm)\r\n",rpm_target/60,rpm_target);
+    m_duty.period(0.00005);
+    encoder_vibrate.init(200,0,t_pid,X4_ENCODE);
+    encoder_outaxis.init(2048,0,t_pid,X4_ENCODE);
+    speedPid.init(Kp,Ki,Kd,t_pid);
+    calc_pid.attach(&duty, t_pid);
+    num_pidticker=0;
+}
+
+/**************
+output_function
+**************/
+void duty(){
+    encoder_vibrate.calculate();
+    encoder_outaxis.calculate();
+    speedPid.cal(rpm_target,encoder_vibrate.getState(RPM));
+    m_duty = speedPid.getState(OUTPUT);
+    m_dir = 1;
+    
+    num_pidticker++;
+    tim_renew ++;
+}
+
+void accelerate(){
+    rpm_target = rpm_target + 150;
+    if(rpm_target > 1500) renew = true;
+}
+
+int main() {
+    //wait(5);
+    setup();
+    while(1) {
+        if(tim_renew >= 1){
+            debug();
+            tim_renew = 0;
+        }
+        
+        if(button == 0){
+            if(button_state == 1){
+                rpm_target += 60*5;
+                setup();
+                wait(0.2);
+            }
+            button_state = 0;//pushed
+        }else{
+            button_state = 1;//not-pushed
+        }
+        //wait(0.5);
+    }
+}
+
+void debug(){
+        sec = num_pidticker*t_pid;
+        pc.printf("%5.2f,%6.2f,%4.2f\r\n",sec,encoder_vibrate.getState(RPM),-encoder_outaxis.getState(RPM));
+}