yuji fujita / Mbed 2 deprecated backdrive_3

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));
}