Department of Electrical Eng University of Moratuwa / Mbed 2 deprecated Theekshana_AI_data

rtos sd encoder systems

Dependencies:   mbed

main.cpp@1:db36f62f783b, 2019-08-19 (annotated)

Committer:
eembed
Date:
Mon Aug 19 18:01:21 2019 +0000
Revision:
1:db36f62f783b
Parent:
0:5459cdde6298
Child:
2:00c6646f42fb
Child:
3:a921792d9913
static friction estimation

Who changed what in which revision?

UserRevisionLine numberNew contents of line
eembed 0:5459cdde6298 1 #include "mbed.h"
eembed 0:5459cdde6298 2 #include "rtos.h"
eembed 0:5459cdde6298 3 #include "SDFileSystem.h"
eembed 0:5459cdde6298 4 #include "qeihw.h"
eembed 1:db36f62f783b 5 //------------------------------------------------------------------------------
eembed 1:db36f62f783b 6 Serial pc(USBTX, USBRX);
eembed 1:db36f62f783b 7 DigitalOut led3(LED3);
eembed 1:db36f62f783b 8 DigitalOut led1(LED1);
eembed 1:db36f62f783b 9 SDFileSystem sd(p5, p6, p7, p8, "sd"); // pintout for sd card
eembed 1:db36f62f783b 10 //-------------------------=Thread Variables=-----------------------------------
eembed 1:db36f62f783b 11 Ticker time_up;
eembed 1:db36f62f783b 12 float startTime=0.0;
eembed 1:db36f62f783b 13 float tempTime = 0.0;
eembed 1:db36f62f783b 14 float endTime=0.0;
eembed 1:db36f62f783b 15 Timer timer;
eembed 1:db36f62f783b 16 float preTime = 0.0;
eembed 1:db36f62f783b 17 //-------------------------=Velocity reading variables=-------------------------
eembed 1:db36f62f783b 18 char buf[0x600];
eembed 1:db36f62f783b 19 float recv_angle = 0.0;
eembed 1:db36f62f783b 20 float x_res = 0.0;
eembed 1:db36f62f783b 21 float dt = 0.000001*150.0;
eembed 1:db36f62f783b 22 float ve_sum = 0.0;
eembed 1:db36f62f783b 23 float dx_res = 0.0;
eembed 1:db36f62f783b 24 float G_filter_v = 100.0;
eembed 1:db36f62f783b 25 float duty = 0.0;
eembed 1:db36f62f783b 26 //-------------------------=Current PID Variables=------------------------------
eembed 1:db36f62f783b 27 float K_pi=25.0;
eembed 1:db36f62f783b 28 float K_di=0.0;
eembed 1:db36f62f783b 29 float K_ii=1.0;
eembed 1:db36f62f783b 30 float I_ref = 0.02;
eembed 1:db36f62f783b 31 float I_error = 0.0;
eembed 1:db36f62f783b 32 float I_PID = 0.0;
eembed 1:db36f62f783b 33 float I_err_sum = 0.0;
eembed 1:db36f62f783b 34 float I_err_prev = 0.0;
eembed 1:db36f62f783b 35 //--------------------------=Motor Variables=-----------------------------------
eembed 1:db36f62f783b 36 DigitalOut Reset_AB(p29);
eembed 1:db36f62f783b 37 DigitalOut Reset_CD(p30);
eembed 1:db36f62f783b 38 PwmOut pwm_clk(p23);
eembed 1:db36f62f783b 39 PwmOut pwm_anticlk(p24);
eembed 1:db36f62f783b 40 //-------------------------=DoB variables=--------------------------------------
eembed 1:db36f62f783b 41 float K_tn=0.135;
eembed 1:db36f62f783b 42 float J_n = 0.0000720;
eembed 1:db36f62f783b 43 float g_dis = 100.0;
eembed 1:db36f62f783b 44 float D_DoB = 0.0;
eembed 1:db36f62f783b 45 float D_BFilter = 0.0;
eembed 1:db36f62f783b 46 float D_AFilter = 0.0;
eembed 1:db36f62f783b 47 //-------------------------=Ethernet Variables=---------------------------------
eembed 0:5459cdde6298 48 Ethernet eth;
eembed 0:5459cdde6298 49
eembed 1:db36f62f783b 50 //-------------------------=Current controller parameters=----------------------
eembed 0:5459cdde6298 51 int Master_Direction=0;
eembed 1:db36f62f783b 52 DigitalIn M_Dir(p10);
eembed 1:db36f62f783b 53 AnalogIn current_sensor_m_p(p17); // current sensor input for MASTER positive
eembed 1:db36f62f783b 54 AnalogIn current_sensor_m_n(p18); // current sensor input for MASTER negative
eembed 1:db36f62f783b 55 float I_res_m = 0.0;
eembed 1:db36f62f783b 56 float I1_act_m=0.0;
eembed 1:db36f62f783b 57 float I_msrd=0.0;
eembed 1:db36f62f783b 58 float G_Cfilter=30.0;
eembed 1:db36f62f783b 59 //---------------------------------=Velocity=-----------------------------------
eembed 1:db36f62f783b 60 void readVelocity(){
eembed 1:db36f62f783b 61 int size2 = eth.receive();
eembed 1:db36f62f783b 62 if(size2 > 0)
eembed 1:db36f62f783b 63 {
eembed 1:db36f62f783b 64 eth.read(buf, size2);
eembed 1:db36f62f783b 65 memcpy(&recv_angle, buf, sizeof(float));
eembed 1:db36f62f783b 66 x_res = -1*recv_angle;
eembed 1:db36f62f783b 67 }
eembed 1:db36f62f783b 68 ve_sum += dx_res*dt;
eembed 1:db36f62f783b 69 dx_res =G_filter_v*( x_res-ve_sum);
eembed 1:db36f62f783b 70 }//-----------------------------=Current Meassurement=--------------------------
eembed 1:db36f62f783b 71 void readCurrent(){
eembed 1:db36f62f783b 72 Master_Direction = M_Dir.read();
eembed 0:5459cdde6298 73
eembed 1:db36f62f783b 74 if(Master_Direction == 0){ //master clockwise
eembed 0:5459cdde6298 75 I_res_m = current_sensor_m_p.read();
eembed 1:db36f62f783b 76 I1_act_m = -1.0*((I_res_m*3.3/0.7) ); //0.74787687701613 //0.717075441532258
eembed 0:5459cdde6298 77
eembed 0:5459cdde6298 78 }else if(Master_Direction == 1) { //master anticlockwise
eembed 0:5459cdde6298 79 I_res_m = current_sensor_m_n.read();
eembed 1:db36f62f783b 80 I1_act_m = 1.0*((I_res_m*3.3)/0.7); //0.713239227822580
eembed 0:5459cdde6298 81 }
eembed 1:db36f62f783b 82 I_msrd += G_Cfilter*(I1_act_m-I_msrd)*dt;
eembed 0:5459cdde6298 83 }
eembed 0:5459cdde6298 84
eembed 1:db36f62f783b 85 //-----------------------------=Current Controller PID=-------------------------
eembed 1:db36f62f783b 86 void currentPID(){
eembed 1:db36f62f783b 87 I_error = I_ref - I_msrd;
eembed 1:db36f62f783b 88 I_err_sum+= (I_error*dt);
eembed 1:db36f62f783b 89 I_PID = (K_pi * I_error)+(K_di*(I_error-I_err_prev)/dt)+(K_ii*I_err_sum);
eembed 1:db36f62f783b 90 I_err_prev = I_error;
eembed 1:db36f62f783b 91 }
eembed 1:db36f62f783b 92 //-----------------------------=Motor PWM Initialization=-----------------------
eembed 1:db36f62f783b 93 void motorPWM_init(){
eembed 1:db36f62f783b 94 pwm_clk.period_us(10);
eembed 1:db36f62f783b 95 pwm_anticlk.period_us(10);
eembed 1:db36f62f783b 96 pwm_clk.write(0.0f);
eembed 1:db36f62f783b 97 pwm_anticlk.write(0.0f);
eembed 1:db36f62f783b 98 Reset_AB = 1;
eembed 1:db36f62f783b 99 Reset_CD = 1;
eembed 1:db36f62f783b 100 }
eembed 1:db36f62f783b 101 //-----------------------------=Motor PWM Generation=---------------------------
eembed 1:db36f62f783b 102 void motorPWM(){
eembed 1:db36f62f783b 103 duty = I_PID;
eembed 1:db36f62f783b 104 if (duty> 0.0) {
eembed 1:db36f62f783b 105 if (duty > 0.5) {
eembed 1:db36f62f783b 106 duty = 0.5;
eembed 1:db36f62f783b 107 }
eembed 1:db36f62f783b 108 pwm_clk = 0.0;
eembed 1:db36f62f783b 109 pwm_anticlk = duty;
eembed 1:db36f62f783b 110 }
eembed 0:5459cdde6298 111
eembed 1:db36f62f783b 112 if (duty < 0.0) {
eembed 1:db36f62f783b 113 if (duty< -0.5) {
eembed 1:db36f62f783b 114 duty = -0.5;
eembed 1:db36f62f783b 115 }
eembed 1:db36f62f783b 116 pwm_anticlk = 0.0;
eembed 1:db36f62f783b 117 pwm_clk = -1.0 * duty;
eembed 1:db36f62f783b 118 }
eembed 1:db36f62f783b 119 }
eembed 1:db36f62f783b 120 //--------------------------=Distarbance Observer=------------------------------
eembed 1:db36f62f783b 121 void DoB(){
eembed 1:db36f62f783b 122 D_BFilter=(I_msrd*K_tn) + (dx_res*J_n*g_dis);
eembed 1:db36f62f783b 123 D_AFilter += g_dis*(D_BFilter-D_AFilter)*dt;
eembed 1:db36f62f783b 124 D_DoB = D_AFilter - (dx_res*J_n*g_dis);
eembed 1:db36f62f783b 125
eembed 1:db36f62f783b 126 }
eembed 1:db36f62f783b 127 //--------------------------=Control Loop=--------------------------------------
eembed 1:db36f62f783b 128 void controlLoop(){
eembed 1:db36f62f783b 129 tempTime = timer.read_us();
eembed 1:db36f62f783b 130 startTime = tempTime-preTime;
eembed 1:db36f62f783b 131 preTime = tempTime;
eembed 1:db36f62f783b 132 readVelocity();
eembed 1:db36f62f783b 133 readCurrent();
eembed 1:db36f62f783b 134 currentPID();
eembed 1:db36f62f783b 135 motorPWM();
eembed 1:db36f62f783b 136 DoB();
eembed 1:db36f62f783b 137
eembed 1:db36f62f783b 138 }
eembed 1:db36f62f783b 139 //------------------------------=File print test=-------------------------------
eembed 1:db36f62f783b 140 void sd_card_write_test()
eembed 1:db36f62f783b 141 {
eembed 1:db36f62f783b 142 mkdir("/sd/mydir", 0777);
eembed 1:db36f62f783b 143
eembed 1:db36f62f783b 144 FILE *fp = fopen("/sd/mydir/sdtest.txt", "w");
eembed 1:db36f62f783b 145 if(fp == NULL) {
eembed 1:db36f62f783b 146 error("Could not open file for write\n");
eembed 1:db36f62f783b 147 }
eembed 1:db36f62f783b 148 fprintf(fp, "Hello fun SD Card World!");
eembed 1:db36f62f783b 149 fclose(fp);
eembed 1:db36f62f783b 150
eembed 1:db36f62f783b 151 //fprintf(fp,"startTime\t I_ref\t I_msrd\t dx_res\t\n");
eembed 1:db36f62f783b 152
eembed 0:5459cdde6298 153 }
eembed 0:5459cdde6298 154
eembed 1:db36f62f783b 155 //----------------------------=File print=--------------------------------------
eembed 1:db36f62f783b 156 void sd_card_write()
eembed 1:db36f62f783b 157 {
eembed 1:db36f62f783b 158 led1=!led1;
eembed 1:db36f62f783b 159 FILE *fp = fopen("/sd/mydir/sdtest.txt","a");
eembed 1:db36f62f783b 160 fprintf(fp,"%f\t %f\t %f\t %f\t\r\n",startTime, I_ref, I_msrd, dx_res);
eembed 1:db36f62f783b 161 fclose(fp);
eembed 0:5459cdde6298 162 }
eembed 0:5459cdde6298 163
eembed 1:db36f62f783b 164 //------------------------=Printing to a file=----------------------------------
eembed 1:db36f62f783b 165 void thread_2(void const *argument)
eembed 1:db36f62f783b 166 {
eembed 1:db36f62f783b 167 while(1)
eembed 1:db36f62f783b 168 {
eembed 1:db36f62f783b 169 //pc.printf("%f %f\n",I1_act_m,I_msrd);
eembed 1:db36f62f783b 170 pc.printf("%f %f %f %f %f %f \n",startTime,I_ref,I_msrd,I_PID,x_res,D_DoB);
eembed 1:db36f62f783b 171 //sd_card_write();
eembed 1:db36f62f783b 172 }
eembed 0:5459cdde6298 173 }
eembed 1:db36f62f783b 174 //-----------------------------=Ethernet Initialization=------------------------
eembed 1:db36f62f783b 175 void ethernet_init()
eembed 1:db36f62f783b 176 {
eembed 1:db36f62f783b 177 eth.set_link(Ethernet::HalfDuplex100);
eembed 1:db36f62f783b 178 wait_ms(1000); // Needed after startup.
eembed 1:db36f62f783b 179 if(eth.link())
eembed 1:db36f62f783b 180 {
eembed 1:db36f62f783b 181 for(int i=0;i<3;i++)
eembed 1:db36f62f783b 182 {
eembed 1:db36f62f783b 183 led3=!led3;
eembed 1:db36f62f783b 184 wait(1.0);
eembed 1:db36f62f783b 185 }
eembed 0:5459cdde6298 186 }
eembed 0:5459cdde6298 187 }
eembed 0:5459cdde6298 188
eembed 1:db36f62f783b 189 //----------------------------------=Main Loop=---------------------------------
eembed 1:db36f62f783b 190 int main()
eembed 1:db36f62f783b 191 {
eembed 1:db36f62f783b 192 //sd_card_write_test();
eembed 1:db36f62f783b 193 ethernet_init();
eembed 1:db36f62f783b 194 motorPWM_init();
eembed 0:5459cdde6298 195 timer.start();
eembed 1:db36f62f783b 196 time_up.attach(&controlLoop, dt);
eembed 0:5459cdde6298 197 Thread thread(*thread_2,NULL,osPriorityAboveNormal,DEFAULT_STACK_SIZE*10.0,NULL);
eembed 1:db36f62f783b 198 }