hidaka sato / Mbed 2 deprecated 2016_slave_MD_rorikon

MD_rorikon_9/30

Dependencies:   PID QEI i2cslave mbed

Files at this revision

API Documentation at this revision

Revision 0:d527838453f6, committed 2016-09-30

Comitter:
sgrsn
Date:
Fri Sep 30 12:31:49 2016 +0000
Commit message:
MD_rorikon_9/30

Changed in this revision

PID.lib Show annotated file Show diff for this revision Revisions of this file
QEI.lib Show annotated file Show diff for this revision Revisions of this file
define.h Show annotated file Show diff for this revision Revisions of this file
i2cslave.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file 
diff -r 000000000000 -r d527838453f6 PID.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.lib	Fri Sep 30 12:31:49 2016 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/users/sgrsn/code/PID/#3ca1603fbcda

diff -r 000000000000 -r d527838453f6 QEI.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.lib	Fri Sep 30 12:31:49 2016 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/users/sgrsn/code/QEI/#b9a5e2d6fbb9

diff -r 000000000000 -r d527838453f6 define.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/define.h	Fri Sep 30 12:31:49 2016 +0000
@@ -0,0 +1,43 @@
+/*Registar group*************************************/
+
+#define motor1_state    0x01
+#define motor2_state    0x02
+#define motor1_pwm      0x03
+#define motor2_pwm      0x04
+#define check_reg       0x05
+#define PM_target_reg   0x10
+#define RE_target_reg   0x20
+#define Kp              0x30
+#define pid_start       0x40
+#define machine_state   0x45
+#define PM_ofset_reg    0x50
+#define wheel_start     0x60
+#define PM_start        0x70
+
+/*Address group*************************************/
+//MD : MotorDriver
+#define MD1_addr 0x12
+#define MD2_addr 0x14
+#define MD3_addr 0x16
+#define MD4_addr 0x18
+#define MD5_addr 0x20
+#define MD6_addr 0x22
+#define MD7_addr 0x24
+#define MD8_addr 0x26
+#define MD9_addr 0x28
+#define MD10_addr 0x30
+#define MD11_addr 0x32
+#define MD12_addr 0x34
+#define MD13_addr 0x36
+#define MD14_addr 0x38
+#define MD15_addr 0x40
+
+//SE : Sensor
+#define SE1_addr 0x90
+#define SE2_addr 0xA0
+
+/*Motor movement***********************************/
+
+#define Nomal 5
+#define Reverse 10
+#define Stop 3

diff -r 000000000000 -r d527838453f6 i2cslave.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/i2cslave.lib	Fri Sep 30 12:31:49 2016 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/sgrsn/code/i2cslave/#69b088a9899b

diff -r 000000000000 -r d527838453f6 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Sep 30 12:31:49 2016 +0000
@@ -0,0 +1,261 @@
+#include "mbed.h"
+#include "QEI.h"
+#include "i2cslave.h"
+#include "PID.h"
+#include "define.h"
+
+#define Enter 13
+#define BackSpace 8
+#define Space 32
+
+union  UNION
+{
+    int integer;
+    char c[6];
+};
+
+UNION PM_target;
+UNION RE_target;
+UNION gain;
+UNION PM_ofset;
+
+char Registar[128] = {};
+QEI rorikon(dp25, dp17, NC, 50, QEI::X4_ENCODING);
+//AnalogIn   potentiometer(dp10);
+AnalogIn   potentiometer(dp11);
+i2cslave myi2c(dp5, dp27, Registar);
+int getmode_PM_angle(const int kaisu);
+int checker = 0;
+void check();
+
+int main()
+{
+    NVIC_SetPriority(EINT3_IRQn, 30);
+    NVIC_SetPriority(EINT2_IRQn, 30);
+    NVIC_SetPriority(EINT1_IRQn, 30);
+    NVIC_SetPriority(EINT0_IRQn, 30);
+    NVIC_SetPriority(TIMER_16_0_IRQn, 20);
+    NVIC_SetPriority(TIMER_16_1_IRQn, 20);
+    NVIC_SetPriority(TIMER_32_0_IRQn, 20);
+    NVIC_SetPriority(TIMER_32_1_IRQn, 20);
+    NVIC_SetPriority(I2C_IRQn, 10);
+    NVIC_SetPriority(UART_IRQn, 5);
+    
+    /*BusOut PM_motor(dp4, dp9, dp13, dp14);
+    BusOut RE_motor(dp25, dp24, dp18, dp17);*/
+    BusOut PM_motor(dp14, dp13, dp10, dp9);
+    BusOut RE_motor(dp28, dp26, dp24, dp18);
+    PwmOut PM_pwm(dp2);
+    PwmOut RE_pwm(dp1);
+    
+    Ticker tic;
+    tic.attach(check, 0.1);
+    Timer t;
+    PID PM_PID(&t);
+    PID RE_PID(&t);
+    PM_PID.set_parameter(0.0200, 0.43);//0.0200, 0.43//0.0200, 0.4   //0.0400, 0.317460317
+    //RE_PID.set_parameter(0.001, 0.07);
+    //RE_PID.set_parameter(0.00070, 0.30);//0.00074, 0.35 //0.00074, 0.277777778  //parameter of 1500
+    //RE_PID.set_parameter(0.00025, 0.37); //0.00040, 0.344827586   //parameter of 300
+    float pm_pid = 0;
+    float re_pid = 0;
+    float RE_angle = 0;
+    float prev_RE_angle = 0;
+    int PM_angle = 0;
+    float nowtime = 0;
+    float prev_time = 0;
+    float RE_speed = 0;
+    int target = 1800;
+    int prev_target = 1800;
+    float Threshold = 5.5;
+    float RE_Threshold = 5;
+    /*****************************************/
+    myi2c.address(MD1_addr);
+    /*****************************************/
+    myi2c.frequency(1000000);
+    
+    PM_ofset.integer = getmode_PM_angle(128);
+    for(int i = 0; i < 4; i++)
+    {
+        Registar[PM_ofset_reg + i] = PM_ofset.c[i];
+    }
+    printf("start");
+    t.start();
+    while(1)
+    {
+        PM_angle = getmode_PM_angle(50);
+        for(int i = 0; i < 5; i++)
+        {
+            PM_target.c[i] = Registar[PM_target_reg + i];
+            RE_target.c[i] = Registar[RE_target_reg + i];
+            gain.c[i] = Registar[Kp + i];
+        }
+        prev_target = target;
+        target = PM_target.integer;
+        /*if(!Registar[Joystick_ctrl] &&(abs((target - PM_ofset.integer) % 45) != 0))
+        {
+            target = prev_target;
+        }*/
+        //printf("%d.%d%d%d", PM_angle, target, (int)(PM_PID.Ki*10000), (int)nowtime);
+        prev_RE_angle = RE_angle;
+        prev_time = nowtime;
+        RE_angle = rorikon.getSumangle();
+        nowtime = t.read();
+        RE_speed = (RE_angle - prev_RE_angle)/(nowtime - prev_time);
+        printf("%f%d%d", RE_speed, gain.integer, (int)nowtime);
+        switch(RE_target.integer)
+        {
+            case 1500:
+                RE_PID.set_parameter(0.00070, 0.30);
+                break;
+            case 300:
+                RE_PID.set_parameter(0.00025, 0.37);
+                break;
+        }
+        printf("%d", Registar[wheel_start]);
+        if(Registar[wheel_start] == 2)
+        {
+            re_pid = RE_PID.PI_lateD((-1 * (float)RE_target.integer), RE_speed);//, ((float)gain.integer)/10000);
+        }
+        else if(Registar[wheel_start] == 1)
+        {
+            re_pid = RE_PID.PI_lateD((float)RE_target.integer, RE_speed);//, ((float)gain.integer)/10000);
+        }
+        /*else if(Registar[pid_start] && ( (RE_speed < (RE_target.integer - RE_Threshold)) || (RE_speed > (RE_target.integer + RE_Threshold)) ) )
+        {
+            re_pid = RE_PID.control_P((float)RE_target.integer, RE_speed, (float)gain.integer/100000);
+        }*/
+        else
+        {
+            re_pid = 0;
+            RE_PID.reset();
+            //pid = mypid.control_P(0, PM_angle, 0.005);
+        }
+        
+        if(re_pid > 0.00005)
+        {
+            RE_motor = 0;
+            wait_ms(5);
+            RE_pwm = re_pid;
+            RE_motor = 10;
+        }
+        else if(re_pid < -0.00005)
+        {
+            RE_motor = 0;
+            wait_ms(5);
+            RE_pwm = re_pid * (-1);
+            RE_motor = 5;
+        }
+        else
+        {
+            RE_motor = 0;
+            wait_ms(5);
+            RE_pwm = 1;
+            RE_motor = 3;
+        }
+        
+        
+        if( ( PM_angle > (target - Threshold) ) && ( PM_angle < (target + Threshold) ) )
+        {
+            pm_pid = 0;
+            PM_PID.reset();
+            PM_motor = 0;
+            wait_ms(1);
+            PM_motor = 3;
+            PM_pwm = 1;
+        }
+        
+        if(Registar[PM_start])
+        {
+            pm_pid = PM_PID.control_P(target, PM_angle, 0.007);
+        }
+        if(Registar[PM_start] && Registar[pid_start] && ( (PM_angle < (target - Threshold)) || (PM_angle > (target + Threshold)) ) )
+        {
+            pm_pid = PM_PID.PI_lateD(target, PM_angle);//, ((float)gain.integer)/10000);
+        }
+        else if(Registar[pid_start])
+        {
+            pm_pid = 0;
+            PM_PID.reset();
+            //pid = mypid.control_P(0, PM_angle, 0.005);
+        }
+        
+        if(pm_pid > 0.04)
+        {
+            PM_motor = 0;
+            wait_ms(1);
+            PM_pwm = pm_pid;
+            PM_motor = 5;
+        }
+        else if(pm_pid < -0.04)
+        {
+            PM_motor = 0;
+            wait_ms(1);
+            PM_pwm = pm_pid * (-1);
+            PM_motor = 10;
+        }
+        else
+        {
+            PM_motor = 0;
+            wait_ms(1);
+            PM_pwm = 1;
+            PM_motor = 3;
+            PM_PID.reset();
+        }
+        printf("\r\n");
+        wait_ms(60);
+    }
+}
+
+int getmode_PM_angle(const int kaisu)
+{
+    int PMangle[128] = {};
+    int samecount[128] = {};
+    int tmpcount[128] = {};
+    int num = 0;
+    for(int i = 0; i < kaisu; i++)
+    {
+        PMangle[i] = (int)(potentiometer * 3600 + 0.5);
+    }
+    for(int i = 0; i < kaisu; i++)
+    {
+        for(int j = 0; j < kaisu; j++)
+        {
+            if(PMangle[i] == PMangle[j])
+            {
+                samecount[i]++;
+            }
+        }
+        tmpcount[i] = samecount[i];
+    }
+    for(int i = 0; i < kaisu; i++)
+    {
+        for(int j = 0; j < i; j++)
+        {
+            if(tmpcount[i] > tmpcount[j])
+            {
+                int tmp = tmpcount[j];
+                tmpcount[j] = tmpcount[i];
+                tmpcount[i] = tmp;
+            }
+        }
+    }
+    int mostcount = tmpcount[0];
+    for(int i = 0; i < kaisu; i++)
+    {
+        if(samecount[i] == mostcount)
+        {
+            num = i;
+        }
+    }
+    return PMangle[num];
+}
+
+void check()
+{
+    Registar[check_reg]++;
+    if(Registar[check_reg] > 2)
+    {
+        NVIC_SystemReset();
+    }
+}
\ No newline at end of file

diff -r 000000000000 -r d527838453f6 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Fri Sep 30 12:31:49 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/b0220dba8be7
\ No newline at end of file