yuji fujita / Mbed 2 deprecated backdrive_3

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@1:dfc7b884cca0, 2020-02-28 (annotated)

Committer:
fuji_
Date:
Fri Feb 28 02:33:37 2020 +0000
Revision:
1:dfc7b884cca0
Parent:
0:25526d7f05c5 
final version of backdrive program in 2020.2.28

Who changed what in which revision?

UserRevisionLine numberNew contents of line
fuji_ 0:25526d7f05c5 1 #include "mbed.h"
fuji_ 0:25526d7f05c5 2 #include "math.h"
fuji_ 0:25526d7f05c5 3 #include "encoder.h"//#include "Encoder.h"//encoder
fuji_ 0:25526d7f05c5 4 #include "pid.h"//position_and_velocity_PID
fuji_ 0:25526d7f05c5 5 #include "core_cm3.h" //NVIC_Setting
fuji_ 0:25526d7f05c5 6 #include "misc.h" //NVIC_Setting
fuji_ 0:25526d7f05c5 7
fuji_ 0:25526d7f05c5 8 #define t_pid 0.5
fuji_ 0:25526d7f05c5 9 #define Kp 0.00
fuji_ 0:25526d7f05c5 10 #define Ki 0.0005
fuji_ 0:25526d7f05c5 11 #define Kd 0.0
fuji_ 0:25526d7f05c5 12 #define pc_tx USBTX
fuji_ 0:25526d7f05c5 13 #define pc_rx USBRX
fuji_ 0:25526d7f05c5 14 #define pc_baud 460800
fuji_ 0:25526d7f05c5 15
fuji_ 0:25526d7f05c5 16 Encoder encoder_vibrate(D11, D12);
fuji_ 0:25526d7f05c5 17 Encoder encoder_outaxis(D13, D14);
fuji_ 0:25526d7f05c5 18 DigitalOut m_dir(D2);
fuji_ 0:25526d7f05c5 19 PwmOut m_duty(D3);
fuji_ 0:25526d7f05c5 20 Serial pc(USBTX,USBRX,460800);
fuji_ 0:25526d7f05c5 21 SpeedPid speedPid;
fuji_ 0:25526d7f05c5 22 Ticker calc_pid;
fuji_ 0:25526d7f05c5 23 DigitalIn button(USER_BUTTON);
fuji_ 0:25526d7f05c5 24
fuji_ 0:25526d7f05c5 25 void debug();
fuji_ 0:25526d7f05c5 26 void duty();
fuji_ 0:25526d7f05c5 27 void accelerate();
fuji_ 0:25526d7f05c5 28 int num_pidticker=0;
fuji_ 0:25526d7f05c5 29 float rpm_target = 0;//inital_value(increasable later)
fuji_ 0:25526d7f05c5 30 float sec=0;
fuji_ 0:25526d7f05c5 31 bool renew = false;
fuji_ 0:25526d7f05c5 32 int tim_renew = 0;
fuji_ 0:25526d7f05c5 33 bool button_state;
fuji_ 0:25526d7f05c5 34
fuji_ 0:25526d7f05c5 35 /*************
fuji_ 0:25526d7f05c5 36 setup_function
fuji_ 0:25526d7f05c5 37 *************/
fuji_ 0:25526d7f05c5 38 void setup() {
fuji_ 0:25526d7f05c5 39 //NVIC Initialize
fuji_ 0:25526d7f05c5 40 NVIC_InitTypeDef NVIC_InitStructure;
fuji_ 0:25526d7f05c5 41 NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
fuji_ 0:25526d7f05c5 42
fuji_ 0:25526d7f05c5 43 //// Enable the GPIO Interrupt for Encorder
fuji_ 0:25526d7f05c5 44 NVIC_InitStructure.NVIC_IRQChannel = EXTI9_5_IRQn;
fuji_ 0:25526d7f05c5 45 NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
fuji_ 0:25526d7f05c5 46 NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
fuji_ 0:25526d7f05c5 47 NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
fuji_ 0:25526d7f05c5 48 NVIC_Init(&NVIC_InitStructure);
fuji_ 0:25526d7f05c5 49
fuji_ 0:25526d7f05c5 50 //// Enable the TIM4 Interrupt
fuji_ 0:25526d7f05c5 51 NVIC_InitStructure.NVIC_IRQChannel = TIM4_IRQn;
fuji_ 0:25526d7f05c5 52 NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
fuji_ 0:25526d7f05c5 53 NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
fuji_ 0:25526d7f05c5 54 NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
fuji_ 0:25526d7f05c5 55 NVIC_Init(&NVIC_InitStructure);
fuji_ 0:25526d7f05c5 56
fuji_ 0:25526d7f05c5 57 //// Change the Systick priority
fuji_ 0:25526d7f05c5 58 NVIC_InitStructure.NVIC_IRQChannel = SysTick_IRQn;
fuji_ 0:25526d7f05c5 59 NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
fuji_ 0:25526d7f05c5 60 NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
fuji_ 0:25526d7f05c5 61 NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
fuji_ 0:25526d7f05c5 62 NVIC_Init(&NVIC_InitStructure);
fuji_ 0:25526d7f05c5 63
fuji_ 0:25526d7f05c5 64
fuji_ 0:25526d7f05c5 65 //// Enable the VCP Interrupt
fuji_ 0:25526d7f05c5 66 NVIC_InitStructure.NVIC_IRQChannel = USB_HP_CAN1_TX_IRQn;
fuji_ 0:25526d7f05c5 67 NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;
fuji_ 0:25526d7f05c5 68 NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
fuji_ 0:25526d7f05c5 69 NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
fuji_ 0:25526d7f05c5 70 NVIC_Init(&NVIC_InitStructure);
fuji_ 0:25526d7f05c5 71
fuji_ 0:25526d7f05c5 72 NVIC_InitStructure.NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn;
fuji_ 0:25526d7f05c5 73 NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;
fuji_ 0:25526d7f05c5 74 NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
fuji_ 0:25526d7f05c5 75 NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
fuji_ 0:25526d7f05c5 76 NVIC_Init(&NVIC_InitStructure);
fuji_ 0:25526d7f05c5 77
fuji_ 0:25526d7f05c5 78 //// Enable the TIM2 Interrupt
fuji_ 0:25526d7f05c5 79 NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQn;
fuji_ 0:25526d7f05c5 80 NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 3;
fuji_ 0:25526d7f05c5 81 NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
fuji_ 0:25526d7f05c5 82 NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
fuji_ 0:25526d7f05c5 83 NVIC_Init(&NVIC_InitStructure);
fuji_ 0:25526d7f05c5 84
fuji_ 0:25526d7f05c5 85
fuji_ 0:25526d7f05c5 86 pc.printf("%4.2f Hz (%6.2f rpm)\r\n",rpm_target/60,rpm_target);
fuji_ 0:25526d7f05c5 87 m_duty.period(0.00005);
fuji_ 0:25526d7f05c5 88 encoder_vibrate.init(200,0,t_pid,X4_ENCODE);
fuji_ 0:25526d7f05c5 89 encoder_outaxis.init(2048,0,t_pid,X4_ENCODE);
fuji_ 0:25526d7f05c5 90 speedPid.init(Kp,Ki,Kd,t_pid);
fuji_ 0:25526d7f05c5 91 calc_pid.attach(&duty, t_pid);
fuji_ 0:25526d7f05c5 92 num_pidticker=0;
fuji_ 0:25526d7f05c5 93 }
fuji_ 0:25526d7f05c5 94
fuji_ 0:25526d7f05c5 95 /**************
fuji_ 0:25526d7f05c5 96 output_function
fuji_ 0:25526d7f05c5 97 **************/
fuji_ 0:25526d7f05c5 98 void duty(){
fuji_ 0:25526d7f05c5 99 encoder_vibrate.calculate();
fuji_ 0:25526d7f05c5 100 encoder_outaxis.calculate();
fuji_ 0:25526d7f05c5 101 speedPid.cal(rpm_target,encoder_vibrate.getState(RPM));
fuji_ 0:25526d7f05c5 102 m_duty = speedPid.getState(OUTPUT);
fuji_ 0:25526d7f05c5 103 m_dir = 1;
fuji_ 0:25526d7f05c5 104
fuji_ 0:25526d7f05c5 105 num_pidticker++;
fuji_ 0:25526d7f05c5 106 tim_renew ++;
fuji_ 0:25526d7f05c5 107 }
fuji_ 0:25526d7f05c5 108
fuji_ 0:25526d7f05c5 109 void accelerate(){
fuji_ 0:25526d7f05c5 110 rpm_target = rpm_target + 150;
fuji_ 0:25526d7f05c5 111 if(rpm_target > 1500) renew = true;
fuji_ 0:25526d7f05c5 112 }
fuji_ 0:25526d7f05c5 113
fuji_ 0:25526d7f05c5 114 int main() {
fuji_ 0:25526d7f05c5 115 //wait(5);
fuji_ 0:25526d7f05c5 116 setup();
fuji_ 0:25526d7f05c5 117 while(1) {
fuji_ 0:25526d7f05c5 118 if(tim_renew >= 1){
fuji_ 0:25526d7f05c5 119 debug();
fuji_ 0:25526d7f05c5 120 tim_renew = 0;
fuji_ 0:25526d7f05c5 121 }
fuji_ 0:25526d7f05c5 122
fuji_ 0:25526d7f05c5 123 if(button == 0){
fuji_ 0:25526d7f05c5 124 if(button_state == 1){
fuji_ 1:dfc7b884cca0 125 rpm_target += 60*5*5;
fuji_ 0:25526d7f05c5 126 setup();
fuji_ 0:25526d7f05c5 127 wait(0.2);
fuji_ 0:25526d7f05c5 128 }
fuji_ 0:25526d7f05c5 129 button_state = 0;//pushed
fuji_ 0:25526d7f05c5 130 }else{
fuji_ 0:25526d7f05c5 131 button_state = 1;//not-pushed
fuji_ 0:25526d7f05c5 132 }
fuji_ 0:25526d7f05c5 133 //wait(0.5);
fuji_ 0:25526d7f05c5 134 }
fuji_ 0:25526d7f05c5 135 }
fuji_ 0:25526d7f05c5 136
fuji_ 0:25526d7f05c5 137 void debug(){
fuji_ 0:25526d7f05c5 138 sec = num_pidticker*t_pid;
fuji_ 0:25526d7f05c5 139 pc.printf("%5.2f,%6.2f,%4.2f\r\n",sec,encoder_vibrate.getState(RPM),-encoder_outaxis.getState(RPM));
fuji_ 0:25526d7f05c5 140 }