Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed pid misc encoder
main.cpp
- Committer:
- fuji_
- Date:
- 2019-12-23
- Revision:
- 0:25526d7f05c5
- Child:
- 1:dfc7b884cca0
File content as of revision 0:25526d7f05c5:
#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));
}