Code giao din visual studio:

using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using ZedGraph;
using System.IO;
using System.IO.Ports;
namespace PID_Controler
{
public partial class Form1 : Form
{
/************************Var**********************/
string Kp_Str;
string Ki_Str;
string Kd_Str;
double Kp, Ki, Kd;
public double time = 0;
double ve = 0;
double set = 0;
int t=1;
string nhan = String.Empty;
//string s = String.Empty;
int TickStart, intMode = 1;
public Form1()
{
InitializeComponent();
}
private void button2_Click(object sender, EventArgs e)
{
string s, s2, s3;
GraphPane mypane = zed.GraphPane;
if (Com.IsOpen)
{
button2.BackColor = Color.LightGreen;
s = txtset.Text + "a";
Com.Write(s);
s2 = txtset2.Text + "b";
Com.Write(s2);
s3 = textBox3.Text + "c";
Com.Write(s3);
double.TryParse(txtset.Text, out set);
}
else
MessageBox.Show("Connect to COM !");
}

private void Com_DataReceived(object sender, System.IO.Ports.SerialDataReceivedEventArgs e)

{
string s;
s = Com.ReadExisting();
for (int i = 0; i < s.Length; i++)
{
if (s[i] != '*')
{
nhan = nhan + s[i];
}
else
{
Display(nhan);
double.TryParse(nhan, out ve);
nhan = "";
s = "";
if(t==0) draw(set, ve);
}
}
/*s = Com.ReadExisting();
//Display(s);
if (s != "")
{
//Display(s);
if (s != "*")
{
nhan = nhan + s;
}
else
{
Display(nhan);
double.TryParse(nhan, out ve);
nhan = "";
s = "";
draw(set, ve);
}
}*/
}
private delegate void DlDisplay(string s);
private void Display(string s)
{
if (txtrec.InvokeRequired)
{
DlDisplay sd = new DlDisplay(Display);
txtrec.Invoke(sd, new object[] { s });
}
else
{
txtrec.Text = s;
}
}
private delegate void DlDisplay1(string s);
private void Display1(string s)
{

if (txtrec.InvokeRequired)
{
DlDisplay1 sd = new DlDisplay1(Display1);
}
else
{
}
}
public void draw(double setpoint, double current)
{
LineItem curve1 = zed.GraphPane.CurveList[0] as LineItem;
LineItem curve2 = zed.GraphPane.CurveList[1] as LineItem;
if ((curve1 == null) || (curve2 == null)) return;
IPointListEdit listset = curve1.Points as IPointListEdit;
IPointListEdit listcurrent = curve2.Points as IPointListEdit;
time = time + 1;
listset.Add(time, setpoint);
listcurrent.Add(time, current);
Scale xScale = zed.GraphPane.XAxis.Scale;
if (time > (xScale.Max - xScale.MajorStep))
{
if (intMode == 1)
{
xScale.Max = time + xScale.MajorStep;// keo qua ben phai
xScale.Min = xScale.Max - 100.0;// 40 la do dai truc x
}
else
{
xScale.Max = time + xScale.MajorStep;// keo qua ben phai
xScale.Min = 0;// 40 la do dai truc x
}
}
zed.AxisChange();
zed.Invalidate();
}
private void button6_Click(object sender, EventArgs e)
{
if (Com.IsOpen)
{
button6.BackColor = Color.LightGreen;
Kp = Convert.ToDouble(textBox2.Text)*1000 ;
Kp_Str = Convert.ToString(Kp) + "p";
Ki = Convert.ToDouble(textBox1.Text)*1000 ;
Ki_Str = Convert.ToString(Ki) + "i";
Kd = Convert.ToDouble(textBox5.Text)*1000 ;
Kd_Str = Convert.ToString(Kd) + "d";
Com.Write(Kp_Str);
Com.Write(Ki_Str);
Com.Write(Kd_Str);
}

else
MessageBox.Show("Connect to COM !");
}
private void button5_Click(object sender, EventArgs e)
{
if (button5.Text == "Connect")
{
if (comboBox2.Text == "") { MessageBox.Show("Choose a COM !"); }
else
{
Com.PortName = comboBox2.Text;
Com.BaudRate = Convert.ToInt32(comboBox1.Text);
Com.ReceivedBytesThreshold = 1;
Com.DataBits = 8;
Com.StopBits = (StopBits)Enum.Parse(typeof(StopBits), "1");
Com.RtsEnable = true;
Com.DtrEnable = true;
Com.Open();
label6.Text = "Connected";
label1.Text = "Connect to " + comboBox2.Text;
label1.BackColor = Color.LightGreen;
button5.Text = "Disconnect";
}
}
else if (button5.Text == "Disconnect")
{
Com.Close();
label1.Text = "Disconnect COM";
label1.BackColor = Color.LightCoral;
button5.Text = "Connect";
label6.Text = "Disconnect";
}
}
private void btmode_Click(object sender, EventArgs e)
{
if (btmode.Text == "Sroll")
{
intMode = 1;
btmode.Text = "Compact";
}
else
{
intMode = 0;
btmode.Text = "Sroll";
}
}
private void button3_Click(object sender, EventArgs e)
{
tabControl1.SelectedTab = tabPage2;
}
private void button7_Click(object sender, EventArgs e)
{
string s4;
if (Com.IsOpen)

{
if (button7.Text == "Start")
{
s4 = "1" + "z";
Com.Write(s4);
button7.BackColor = Color.LightCoral;
button7.Text = "Stop";
button2_Click(sender, e);
button6_Click(sender, e);
}
else
{
s4 = "0" + "z";
Com.Write(s4);
button7.BackColor = Color.LightGreen;
button7.Text = "Start";
}
}
else
MessageBox.Show("Connect to COM !");
}
private void Form1_Load(object sender, EventArgs e)
{
tabControl1.SelectedTab = tabPage2;
// khi khi ng s c chy
GraphPane mypane = zed.GraphPane; // Khai bo sa dng Graph loi GraphPane;
// Cc thng tin cho th ca mnh
mypane.Title.Text = "DC Motor Control";
mypane.XAxis.Title.Text = "Time(50mS)";
mypane.YAxis.Title.Text = "Position(Dev)";
// nh ngha list v th. cc bn hiu r c ch lm vic y khai bo 2 list im <=> 2 ng
th
RollingPointPairList listSetPitch = new RollingPointPairList(60000);
//Timer th hin y l c.
// y s dng list vi 60000 im (c th thm nhiu liu ti y)
RollingPointPairList listCurPitch = new RollingPointPairList(60000);
// dng di l nh ngha curve v.
LineItem curveSetPitch = mypane.AddCurve("Position Set", listSetPitch, Color.Red, SymbolType.None); //
Color mu , c trng cho ng 1
// SymbolType l kiu biu th th : im, ng trn, tam gic ....
LineItem curveCurPitch = mypane.AddCurve("Position Now", listCurPitch, Color.Blue, SymbolType.None);
// Color mu Xanh, c trng cho ng 2
// v d khong cch l 50ms 1 ln
//timer1.Interval = 50;
//timer1.Enabled = true; // Kch hot cho timer1
//timer1.Start(); // Chy Timer1
// nh hin th cho trc thi gian (Trc X)
mypane.XAxis.Scale.Min = 0; // Min = 0;
mypane.XAxis.Scale.Max = 20; // Max = 20;
mypane.XAxis.Scale.MinorStep = 1; // n v chia nh nht 1
mypane.XAxis.Scale.MajorStep = 1; // n v chia ln 5
/*
// nh hin th cho trc thi gian (Trc Y)
mypane.YAxis.Scale.Min = -1000; // Min = -90;
mypane.YAxis.Scale.Max = 1000; // Max = 90;*/

mypane.YAxis.Scale.MinorStep = 5; // n v chia nh nht 1

mypane.YAxis.Scale.MajorStep = 10; // n v chia ln 5
// Gi hm xc nh c trc*/
zed.AxisChange();
TickStart = Environment.TickCount;
/////////////////////////////////////////////////////////BAUDRATE
comboBox1.Items.Add(300);
comboBox1.Items.Add(600);
comboBox1.Items.Add(1200);
comboBox1.Items.Add(2400);
comboBox1.Items.Add(9600);
comboBox1.Items.Add(14400);
comboBox1.Items.Add(19200);
comboBox1.Items.Add(38400);
comboBox1.Items.Add(57600);
comboBox1.Items.Add(115200);
comboBox1.Items.ToString();
comboBox1.Text = comboBox1.Items[8].ToString();
///////////////////////////////////////////////////////thiet lap doc com
string[] ports = SerialPort.GetPortNames();
for (int i = 0; i < ports.Length; i++)
{
comboBox2.Items.Add(ports[i]);
}
//comboBox2.Text = comboBox2.Items[0].ToString();
/*********************************************************************************************
********/
//button5_Click(sender, e);
button7.Focus();
}
private void button9_Click(object sender, EventArgs e)
{
double xxx;
xxx = Convert.ToDouble(txtset2.Text);
if (0==0)
{
if (button9.Text == "Pst")
{
txtset.Text = "99999999";
button9.Text = "Veloc";
txtset.Visible = false;
label3.Visible = false;
}
else
{
txtset.Text = "0";
txtset.Visible = true;
label3.Visible = true;
button9.Text = "Pst";
}
}
else

MessageBox.Show("Connect to COM !");

}
private void button8_Click(object sender, EventArgs e)
{
if (button8.Text == "Stop Draw" && Com.IsOpen)
{
//button8.BackColor = Color.LightCoral;
t = 1;
button8.Text = "Draw";
}
else if (!Com.IsOpen)
MessageBox.Show("Connect to COM !");
else
{
t = 0;
//button8.BackColor = Color.LightGreen;
button8.Text = "Stop Draw";
}
}
private void txtset_TextChanged(object sender, EventArgs e)
{
button2.BackColor = Color.LightCoral;
}
private void txtset2_TextChanged(object sender, EventArgs e)
{
button2.BackColor = Color.LightCoral;
}
private void textBox3_TextChanged(object sender, EventArgs e)
{
button2.BackColor = Color.LightCoral;
}
private void textBox2_TextChanged(object sender, EventArgs e)
{
button6.BackColor = Color.LightCoral;
}
private void textBox1_TextChanged(object sender, EventArgs e)
{
button6.BackColor = Color.LightCoral;
}
private void textBox5_TextChanged(object sender, EventArgs e)
{
button6.BackColor = Color.LightCoral;
}
private void txtset_KeyDown(object sender, KeyEventArgs e)
{
if (e.KeyCode==Keys.Enter)
{
button2_Click(sender, e);
}
if (e.KeyCode == Keys.Up)
{
button2.Focus();
}
if (e.KeyCode == Keys.Down)
{
txtset2.Focus();

}
}
private void txtset2_KeyDown(object sender, KeyEventArgs e)
{
if (e.KeyCode == Keys.Enter)
{
button2_Click(sender, e);
}
if (e.KeyCode == Keys.Up)
{
txtset.Focus();
}
if (e.KeyCode == Keys.Down)
{
textBox3.Focus();
}
}
private void textBox3_KeyDown(object sender, KeyEventArgs e)
{
if (e.KeyCode == Keys.Enter)
{
button2_Click(sender, e);
}
if (e.KeyCode == Keys.Up)
{
txtset2.Focus();
}
if (e.KeyCode == Keys.Down)
{
button2.Focus();
}
}
private void textBox2_KeyDown(object sender, KeyEventArgs e)
{
if (e.KeyCode == Keys.Enter)
{
button6_Click(sender, e);
}
if (e.KeyCode == Keys.Up)
{
button6.Focus();
}
if (e.KeyCode == Keys.Down)
{
textBox1.Focus();
}
}
private void textBox1_KeyDown(object sender, KeyEventArgs e)
{
if (e.KeyCode == Keys.Enter)
{
button6_Click(sender, e);
}
if (e.KeyCode == Keys.Up)
{
textBox2.Focus();

}
if (e.KeyCode == Keys.Down)
{
textBox5.Focus();
}
}
private void textBox5_KeyDown(object sender, KeyEventArgs e)
{
if (e.KeyCode == Keys.Enter)
{
button6_Click(sender, e);
}
if (e.KeyCode == Keys.Up)
{
textBox1.Focus();
}
if (e.KeyCode == Keys.Down)
{
button6.Focus();
}
}
private void button2_KeyUp(object sender, KeyEventArgs e)
{
if (e.KeyCode == Keys.Up)
{
textBox3.Focus();
}
if (e.KeyCode == Keys.Down)
{
txtset.Focus();
}
}
private void button6_KeyUp(object sender, KeyEventArgs e)
{
if (e.KeyCode == Keys.Up)
{
textBox5.Focus();
}
if (e.KeyCode == Keys.Down)
{
textBox2.Focus();
}
}
private void button10_Click(object sender, EventArgs e)
{
string s;
if (Com.IsOpen)
{
s = "t";
Com.Write(s);
}
else
MessageBox.Show("Connect to COM !");
}
private void button11_Click(object sender, EventArgs e)

{
time = 0;
LineItem curve1 = zed.GraphPane.CurveList[0] as LineItem;
LineItem curve2 = zed.GraphPane.CurveList[1] as LineItem;
if ((curve1 == null) || (curve2 == null)) return;
IPointListEdit listset = curve1.Points as IPointListEdit;
IPointListEdit listcurrent = curve2.Points as IPointListEdit;
listset.Clear();
listcurrent.Clear();
Scale xScale = zed.GraphPane.XAxis.Scale;
xScale.Max = 100;
xScale.Min = 0;
zed.AxisChange();
zed.Invalidate();
}
private void button13_Click(object sender, EventArgs e)
{
tabControl1.SelectedTab = tabPage2;
}
private void button1_Click(object sender, EventArgs e)
{
if (Com.IsOpen)
{
Com.Close();
}
this.Close();
}

private void button4_Click(object sender, EventArgs e)

{
tabControl1.SelectedTab = tabPage3;
}
private void button12_Click(object sender, EventArgs e)
{
tabControl1.SelectedTab = tabPage1;
}
private void button14_Click(object sender, EventArgs e)
{
tabControl1.SelectedTab = tabPage1;
}
private void button15_Click(object sender, EventArgs e)
{
string q;
if (Com.IsOpen)
{
if (button15.Text == "mas_sla")

{
q = "1" + "q";
Com.Write(q);
button15.BackColor = Color.LightCoral;
button15.Text = "sla_mas";
}
else
{
q = "0" + "q";
Com.Write(q);
button15.BackColor = Color.LightGreen;
button15.Text = "mas_sla";
}
}
else
MessageBox.Show("Connect to COM !");
}
}
}
Code ARM master:
#define constrain(cur, low, high) ((cur) < (low) ? (low) : ((cur) > (high) ? (high) : (cur))) ///Limit
//////////////////////////////value
int onoff ;
//float a,b;
// PID
signed long P_old, P_a , P_leg;
signed long P_set,p_set1,P_now,V_set,V_now,A_set;
signed long Up,Ui,Ud;
signed long U_now,U_old;
signed long er_0,er_1,er_2;
signed long er_sum,anti_windup;
float Kp,Ki,Kd,Kb;
signed long encoder;
int sw;
// UART
signed long i,j,k;
char data_rx;
char output[10];
char ptr;
//Encoder
signed long dev_encoder;
unsigned long dev_en_motor;
unsigned int pwm_period;
//CAN
unsigned long Can_Init_Flags;
unsigned char Can_Send_Flags, Can_Rcv_Flags; // can flags
unsigned char Rx_Data_Len;
// received data length in bytes
char RxTx_Data[15];
// can rx/tx data buffer
char Msg_Rcvd;
// reception flag
const long ID_1st = 12111, ID_2nd = 3;
// node IDs
long Rx_ID;
/////////////funtion////////

void init(void);
void read_encoder(void);
void set_pwm(signed long value_pwm);
void uart_send(signed long data_uart);
void uart_recieved(void);
void dels(void);
void dl(int delay);
void PID_control(void);
void main() {
////////////set value///////
onoff = 0;
P_old = P_a = 0;
P_set = P_now = V_set = V_now = A_set = 0;
Up = Ui = Ud = 0;
U_now = U_old = 0;
er_0 = er_1 = er_2 = 0;
er_sum = anti_windup = 0;
Kp = 70.0;
Ki = 0.0;
Kd = 10.0;
Kb = 0.012;
dev_encoder=0;
// So xung doc ve
dev_en_motor=100; // So xung encoder dong co
//UART
i=j=k=0;
data_rx="";
ptr=0;
init();
RxTx_Data[0]=0;
CAN1Write(ID_1st, RxTx_Data, 15, Can_Send_Flags);
while (1) {
char ws[15], Wr[15];
Msg_Rcvd = CAN1Read(0, &Rx_ID , wr, &Rx_Data_Len, &Can_Rcv_Flags); // receive message
j++;
if ((Rx_ID == ID_2nd) && Msg_Rcvd) {
GPIOA_ODR.B4=1;
P_set1=atol(wr);
if(sw==1)
{
ws[0]=10;
Delay_ms(1);
CAN1Write(ID_1st, ws, 15, Can_Send_Flags);
if(j==60)
{
uart_send(dev_encoder);
j=0;
}
}
else
{
read_encoder();
encoder= dev_encoder * 1000;
LongToStr(encoder,ws);
Delay_ms(1);

CAN1Write(ID_1st, ws, 15, Can_Send_Flags);

if(j==60)
{
uart_send(dev_encoder);
j=0;
}
}

// send incremented data back

}
}
}
/*{}*/
void init(void){
////////////////////CAN/////////////////////
Can_Init_Flags = 0;
//
Can_Send_Flags = 0;
// clear flags
Can_Rcv_Flags = 0;
//
Can_Send_Flags = _CAN_TX_XTD_FRAME &
// with CANWrite
_CAN_TX_NO_RTR_FRAME;
Can_Init_Flags = _CAN_CONFIG_AUTOMATIC_RETRANSMISSION &
// form value to be used
_CAN_CONFIG_RX_FIFO_NOT_LOCKED_ON_OVERRUN &
// with CANInit
_CAN_CONFIG_TIME_TRIGGERED_MODE_DISABLED &
_CAN_CONFIG_TX_FIFO_PRIORITY_BY_IDINTIFIER &
_CAN_CONFIG_WAKE_UP;
CAN1InitializeAdvanced(1,5,4,4,1,Can_Init_Flags, &_GPIO_MODULE_CAN1_PA11_12); // Initialize CAN
module
CAN1SetOperationMode(_CAN_OperatingMode_Initialization);
// set CONFIGURATION mode
CANSetFilterScale32(0, _CAN_FILTER_ENABLED & _CAN_FILTER_ID_MASK_MODE &
_CAN_FILTER_XTD_MSG, ID_2nd, -1);
CAN1SetOperationMode(_CAN_OperatingMode_Normal); // set NORMAL mode
/////////////Encoder config/////
RCC_APB2ENR.IOPBEN = 1;
// GPIOB.1
GPIOB_CRLbits.MODE0 = 0;
GPIOB_CRLbits.CNF0 = 0b10;
GPIOB_ODRbits.ODR0 = 1;
// GPIOB.2
GPIOB_CRLbits.MODE1 = 0;
GPIOB_CRLbits.CNF1 = 0b10;
GPIOB_ODRbits.ODR1 = 1;
// TIMER4
RCC_APB1ENR.TIM4EN = 1;
TIM4_CCMR1_Inputbits.CC1S = 1 ;
TIM4_CCMR1_Inputbits.CC2S = 1 ;
TIM4_CCERbits.CC1P = 0 ;
TIM4_CCERbits.CC2P = 0 ;
TIM4_SMCRbits.SMS = 0b011 ;
TIM4_ARR = 65535;
NVIC_IntEnable(IVT_INT_TIM4);
TIM4_DIER.UIE = 1;
TIM4_CR1.EN = 1;
TIM4_CNT=30000;
/////////////IO config/////
GPIO_Digital_Output(&GPIOB_ODR, _GPIO_PINMASK_0);
GPIO_Digital_Output(&GPIOB_BASE , _GPIO_PINMASK_8 | _GPIO_PINMASK_9);

GPIOB_ODR.B0 = 0;
GPIO_Digital_Output(&GPIOA_BASE, _GPIO_PINMASK_4 | _GPIO_PINMASK_5);
GPIOA_ODR.B4 = 0;
/////////////Timer config/////
RCC_APB2ENR.TIM1EN = 1;
// Cap xung cho TIM1, TIM1 nhan xung tu APB2 (trang 70
RM0041_CD00246267)
TIM1_CR1.CEN = 0;
TIM1_PSC = 3599;
TIM1_ARR = 9;
//500uS
NVIC_IntEnable(IVT_INT_TIM1_UP);
TIM1_DIER.UIE = 1;
/////////////PWM config/////
pwm_period = PWM_TIM2_Init(5000);
PWM_TIM2_Set_Duty(0, _PWM_NON_INVERTED, _PWM_CHANNEL4);
PWM_TIM2_Start(_PWM_CHANNEL4, &_GPIO_MODULE_TIM2_CH4_PA2);
/////////////Uart config////
UART1_Init(57600);
EnableInterrupts();
RXNEIE_USART1_CR1_bit = 1;
//enable Receiver Interrupt
NVIC_IntEnable(IVT_INT_USART1); // set Interrup Vector
Delay_ms(100);
UART1_Write_Text("UART Ready!");
UART1_Write_Text ("\n");
}
void set_pwm(signed long value_pwm){
if(value_pwm<0){
value_pwm=(-1)*value_pwm;
GPIOB_ODR.B0=1;
}
else {
GPIOB_ODR.B0=0;
}
if(value_pwm>14000){value_pwm=14000;}
PWM_TIM2_Set_Duty(value_pwm, _PWM_NON_INVERTED, _PWM_CHANNEL4);
}
void usart_RX() iv IVT_INT_USART1 ics ICS_AUTO{
data_rx= UART1_Read();
uart_recieved();
}
void Timer1_interrupt() iv IVT_INT_TIM1_UP ics ICS_AUTO {
signed long x=0,y=0,z=0;
TIM1_SR.UIF = 0;
k++;
if(k==20){
if(P_leg > 0){
x = P_old + (P_leg/2);
if(P_now < x){
if(V_now<V_set){
V_now=constrain(V_now+A_set,0,V_set);
if(V_now >= (V_set-A_set)) { P_a = P_now - P_old; }

else;
}
else;
}
else if(P_now > (x+P_leg/100)){
y = P_set - P_a;
if(P_now > y){
V_now=constrain(V_now-A_set,0,V_set);
}
else if(P_a == 0){
V_now=constrain(V_now-A_set,0,V_set);
}
else;
}
}
else if(P_leg < 0){
x = P_old + (P_leg/2);
if(P_now > x){
if(V_now<V_set){
V_now=constrain(V_now+A_set,0,V_set);
if(V_now >= (V_set-A_set)) { P_a = -P_now + P_old; }
else;
}
else;
}
else if(P_now < (x+P_leg/100)){
y = P_set + P_a;
if(P_now < y){
V_now=constrain(V_now-A_set,0,V_set);
}
else if(P_a == 0){
V_now=constrain(V_now-A_set,0,V_set);
}
else;
}
}
else;
if(P_now<P_set){
P_now=constrain(P_now+V_now,-9999999,P_set);
}
else if(P_now>P_set){
P_now=constrain(P_now-V_now,P_set,9999999);
}
else if(P_now == P_set) {V_now=0; P_a = 0; P_leg = 0;}
k=0;
}
PID_control();
}
void Timer4_interrupt() iv IVT_INT_TIM4 ics ICS_AUTO {
TIM4_SR.UIF = 0;
TIM4_CNT=30000;
}
void read_encoder(void) {
dev_encoder= TIM4_CNT -30000 + dev_encoder;

TIM4_CNT=30000;
}
void uart_send(signed long data_uart) {
char rr[5];
LongToStr(data_uart,rr);
UART1_Write_text(rr);
// and send data via UART
UART1_Write_text("*");
}
void uart_recieved(){
if(data_rx=='z'){
if(atoi(output) == 1) { TIM1_CR1.CEN = 1; }
else { TIM1_CR1.CEN = 0; set_pwm(0);}
dels();
}
else if(data_rx=='q'){
if(atoi(output) == 1) { sw = 1; }
else { sw = 0;}
dels();
}
else if(data_rx== 'a' ){
P_old = P_set;
P_set = atol(output); dels();
P_leg = P_set - P_old;
}
else if(data_rx== 'b' ){
V_set = constrain(atol(output),0,20000); dels();
}
else if(data_rx== 'c' ){
A_set = constrain(atol(output),0,20000); dels();
}
else if(data_rx== 'p' ){
Kp =((float)atol(output))/1000;dels();
}
else if(data_rx== 'i' ){
Ki =((float)atol(output))/1000; dels();
}
else if(data_rx== 'd' ){
Kd =((float)atol(output))/1000; dels();
}
else {
output[ptr++] = data_rx;
}
}
void dels(void){
for (ptr=0;ptr<10;ptr++){
output[ptr]=0;
}
ptr=0;
}
void PID_control(void){
read_encoder();
if(sw==0)
{
er_0=P_now-dev_encoder;
}
else

{
er_0=P_set1-dev_encoder;
}
er_sum=er_sum+er_0+anti_windup;
er_sum=constrain(er_sum,-999999,999999);
Up=Kp*er_0;
Ui=Ki*er_sum;
Ud=Kd*(er_1-er_0);
U_now=Up+Ui+Ud;
if(U_now>14000){ anti_windup=(14000-U_now)*Kb; U_now=14000; }
else if(U_now<-14000){ anti_windup=(-14000-U_now)*Kb; U_now=-14000;}
else {anti_windup=0;}
set_pwm(U_now);
er_1=er_0;
}
Code ARM slave:
//Code ARM Slave:
#define constrain(cur, low, high) ((cur) < (low) ? (low) : ((cur) > (high) ? (high) : (cur))) ///Limit
int onoff;
// PID
signed long P_set,V_set;
signed long Up,Ui,Ud;
signed long U_now,U_old;
signed long er_0,er_1,er_2;
signed long er_sum,anti_windup;
float Kp,Ki,Kd,Kb;
long encoder;
// UART
signed long i,j,k;
char data_rx;
char output[10];
char ptr;
//Encoder
signed long dev_encoder;
unsigned long dev_en_motor;
unsigned int pwm_period;
//CAN
unsigned long Can_Init_Flags;
unsigned char Can_Send_Flags, Can_Rcv_Flags; // can flags
unsigned char Rx_Data_Len;
// received data length in bytes
char RxTx_Data[15];
// can rx/tx data buffer
char Msg_Rcvd;
// reception flag
const long ID_1st = 12111, ID_2nd = 3;
// node IDs
long Rx_ID;
void init(void);
void read_encoder(void);
void set_pwm(signed long value_pwm);
void uart_send(signed long data_uart);
void uart_recieved(void);
void dels(void);
void PID_control(void);
void main() {

////////////set value///////
onoff=0;
P_set = V_set = 0;
Up = Ui = Ud = 0;
U_now = U_old = 0;
er_0 = er_1 = er_2 = 0;
er_sum = anti_windup = 0;
Kp = 70.0;
Ki = 0.2;
Kd = 2.0;
Kb = 0.012;
//Encoder
dev_encoder=0;
// So xung doc ve
dev_en_motor=100; // So xung encoder dong co
//UART
i=j=k=0;
//data_rx=;
ptr=0;
init();
RxTx_Data[0]=0;
while (1) {
char wr [15],ws[15];
Msg_Rcvd = CAN1Read(0, &Rx_ID , wr, &Rx_Data_Len, &Can_Rcv_Flags); // receive message
if ((Rx_ID == ID_1st) && Msg_Rcvd) {
// if message received check id
GPIOA_ODR.B4 = 1;
if(wr[0]==10)
{
TIM1_CR1.CEN = 0 ;
read_encoder();
encoder= dev_encoder * 1000;
LongToStr(encoder,ws);
Delay_ms(1);
CAN1Write(ID_2nd,ws, 15, Can_Send_Flags);
}
else{
TIM1_CR1.CEN = 1 ;
P_set=atol(wr);
Delay_ms(1);
// increment received data
CAN1Write(ID_2nd,RxTx_Data, 15, Can_Send_Flags);
// send incremented data back
}
}
}
}
/*{}*/
void init(void){
//CAN////////////////////////////////
Can_Init_Flags = 0;
//
Can_Send_Flags = 0;
// clear flags
Can_Rcv_Flags = 0;
//
Can_Send_Flags = _CAN_TX_XTD_FRAME &

//

with CANWrite

_CAN_TX_NO_RTR_FRAME;
Can_Init_Flags = _CAN_CONFIG_AUTOMATIC_RETRANSMISSION &
// form value to be used
_CAN_CONFIG_RX_FIFO_NOT_LOCKED_ON_OVERRUN & // with CANInit
_CAN_CONFIG_TIME_TRIGGERED_MODE_DISABLED &
_CAN_CONFIG_TX_FIFO_PRIORITY_BY_IDINTIFIER &
_CAN_CONFIG_WAKE_UP;
CAN1InitializeAdvanced(1,5,4,4,1,Can_Init_Flags, &_GPIO_MODULE_CAN1_PA11_12); // Initialize CAN
module
CAN1SetOperationMode(_CAN_OperatingMode_Initialization);
// set CONFIGURATION mode
CANSetFilterScale32(0, _CAN_FILTER_ENABLED & _CAN_FILTER_ID_MASK_MODE &
_CAN_FILTER_XTD_MSG, ID_1st, -1);
CAN1SetOperationMode(_CAN_OperatingMode_Normal);
// set NORMAL mode
/////////////Encoder config/////
RCC_APB2ENR.IOPBEN = 1;
// GPIOB.1
GPIOB_CRLbits.MODE0 = 0;
GPIOB_CRLbits.CNF0 = 0b10;
GPIOB_ODRbits.ODR0 = 1;
// GPIOB.2
GPIOB_CRLbits.MODE1 = 0;
GPIOB_CRLbits.CNF1 = 0b10;
GPIOB_ODRbits.ODR1 = 1;
// TIMER4
RCC_APB1ENR.TIM4EN = 1;
TIM4_CCMR1_Inputbits.CC1S = 1 ;
TIM4_CCMR1_Inputbits.CC2S = 1 ;
TIM4_CCERbits.CC1P = 0 ;
TIM4_CCERbits.CC2P = 0 ;
TIM4_SMCRbits.SMS = 0b011 ;
TIM4_ARR = 65535;
NVIC_IntEnable(IVT_INT_TIM4);
TIM4_DIER.UIE = 1;
TIM4_CR1.EN = 1;
TIM4_CNT=30000;
/////////////IO config/////
GPIO_Digital_Output(&GPIOB_ODR, _GPIO_PINMASK_0);
GPIO_Digital_Output(&GPIOB_BASE , _GPIO_PINMASK_8 | _GPIO_PINMASK_9);
GPIOB_ODR.B0 = 0;
GPIO_Digital_Output(&GPIOA_BASE, _GPIO_PINMASK_4 );
GPIOA_ODR.B4 = 0;
/////////////Timer config/////
RCC_APB2ENR.TIM1EN = 1;
// Cap xung cho TIM1, TIM1 nhan xung tu APB2 (trang 70
RM0041_CD00246267)
TIM1_CR1.CEN = 0;
TIM1_PSC = 3599;
TIM1_ARR = 9;
//500uS
NVIC_IntEnable(IVT_INT_TIM1_UP);
TIM1_DIER.UIE = 1;
TIM1_CR1.CEN = 1;
/////////////PWM config/////
pwm_period = PWM_TIM2_Init(5000);
PWM_TIM2_Set_Duty(0,_PWM_NON_INVERTED,_PWM_CHANNEL4);
PWM_TIM2_Start(_PWM_CHANNEL4, &_GPIO_MODULE_TIM2_CH4_PA2);

}
void set_pwm(signed long value_pwm){
if(value_pwm<0){
value_pwm=(-1)*value_pwm;
GPIOB_ODR.B0=1;
}
else {
GPIOB_ODR.B0=0;
}
if(value_pwm>14000){value_pwm=14000;}
PWM_TIM2_Set_Duty(value_pwm,_PWM_NON_INVERTED,_PWM_CHANNEL4);
}
void Timer1_interrupt() iv IVT_INT_TIM1_UP ics ICS_AUTO {
TIM1_SR.UIF = 0;
PID_control();
}
void Timer4_interrupt() iv IVT_INT_TIM4 ics ICS_AUTO {
TIM4_SR.UIF = 0;
TIM4_CNT=30000;
}
void read_encoder(void) {
dev_encoder= TIM4_CNT - 30000 + dev_encoder;
TIM4_CNT=30000;
}
void dels(void){
for (ptr=0;ptr<10;ptr++){
output[ptr]=0;
}
ptr=0;
}
void PID_control(void){
read_encoder();
er_0=P_set-dev_encoder;
er_sum=er_sum+er_0+anti_windup;
er_sum=constrain(er_sum,-10000000,10000000);
Up=Kp*er_0;
Ui=Ki*er_sum;
Ud=Kd*(er_1-er_0);
U_now=Up+Ui+Ud;
if(U_now>14000){ anti_windup=(14000-U_now)*Kb; U_now=14000; }
else if(U_now<-14000){ anti_windup=(-14000-U_now)*Kb; U_now=-14000;}
else {anti_windup=0;}
set_pwm(U_now);
er_1=er_0;
}