Revision 0:79ed3a6c9ccc, committed 2015-02-10

Comitter:

romankrej

Date:

Tue Feb 10 14:31:40 2015 +0000

Commit message:

version 2

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/angle.cpp	Tue Feb 10 14:31:40 2015 +0000
@@ -0,0 +1,41 @@
+#include "mbed.h"
+#include "rtos.h"
+#include "angle.h"
+
+char i;
+
+//konstruktor
+IRsensor::IRsensor(PinName IRpin) : IRsens(IRpin) {}
+
+
+//vraci uhel micku na kole
+float IRsensor::get_angle(void)
+{    
+    if(i != 20) {
+        phi_new = IRsens.read();
+        //phi_new = (-768.82*phi_new*phi_new*phi_new + 1756*phi_new*phi_new - 1381*phi_new + 359.14)*0.01745;
+        //phi_new = (318.79*phi_new*phi_new - 455.26*phi_new + 142.85)*0.01745;
+        phi_new = (-445.55*phi_new*phi_new*phi_new + 891.29*phi_new*phi_new - 627.67*phi_new + 143.9)*0.01745;
+        s_phi[7] = s_phi[6]; s_phi[6] = s_phi[5];
+        s_phi[5] = s_phi[4]; s_phi[4] = s_phi[3];
+        s_phi[3] = s_phi[2]; s_phi[2] = s_phi[1];
+        s_phi[1] = s_phi[0]; s_phi[0] = phi_new;   
+    }
+        
+    if(i == 20) {
+        phi_new = IRsens.read();
+        //phi_new = (-768.82*phi_new*phi_new*phi_new + 1756*phi_new*phi_new - 1381*phi_new + 359.14)*0.01745;
+        //phi_new = (318.79*phi_new*phi_new - 455.26*phi_new + 142.85)*0.01745;
+        phi_new = (-445.55*phi_new*phi_new*phi_new + 891.29*phi_new*phi_new - 627.67*phi_new + 143.9)*0.01745;
+        s_phi[0] = phi_new;  s_phi[1] = s_phi[0];
+        s_phi[2] = s_phi[1]; s_phi[3] = s_phi[2];
+        s_phi[4] = s_phi[3]; s_phi[5] = s_phi[4];
+        s_phi[6] = s_phi[5]; s_phi[7] = s_phi[6];
+        i = 0;
+    }
+        
+    phi_new = (s_phi[0] + s_phi[1] + s_phi[2] + s_phi[3] + s_phi[4] + s_phi[5] + s_phi[6] + s_phi[7])/8.0;
+
+    return phi_new;
+    
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/angle.h	Tue Feb 10 14:31:40 2015 +0000
@@ -0,0 +1,16 @@
+#ifndef _ANGLE_H
+#define _ANGLE_H
+extern char i;
+
+class IRsensor { 
+
+    AnalogIn IRsens;
+    float phi_new;
+    float s_phi[8];
+    
+public:
+    IRsensor(PinName IRpin);
+    float get_angle(void);
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/can.cpp	Tue Feb 10 14:31:40 2015 +0000
@@ -0,0 +1,176 @@
+#include "mbed.h"
+#include "rtos.h"
+#include "can.h"
+
+float current;
+int vel;
+int mode;
+float rSpeed;
+float mSpeed;
+char data[4],address[2],data_size,cmd;
+
+void neco(void) {
+
+}
+CANMessage canmsg;
+
+CANbus::CANbus(PinName CANpin1, PinName CANpin2) : Can(CANpin1,CANpin2) {
+    Can.frequency(500000);
+    Can.attach(this,&CANbus::CAN_frame_received);
+}
+
+//vysilani synchronizace
+void CANbus::servo_sync(void) {
+    Can.write(CANMessage(0x80));
+}
+
+int CANbus::get_data(char data[], char size) {
+    int ret_data = 0;
+    for(int i = 0; i < 2*size; i++) {
+        ret_data |= data[i] << 8*i; 
+    }
+    return ret_data;
+}
+
+//posila SDO zadost
+int CANbus::send_SDO_request(char cmd, int adr, char size,int data) {
+    char data_send[8];
+    data_send[0] = cmd;
+    data_send[1] = 0xFF & adr;;
+    data_send[2] = (0xFF00 & adr) >> 8;
+    data_send[3] = size;
+    data_send[4] = 0xFF & data;
+    data_send[5] = (0xFF00 & data) >> 8;
+    data_send[6] = (0xFF0000 & data)>>16;
+    data_send[7] = (0xFF000000 & data)>>24;
+    if(Can.write(CANMessage(SDO_REQUEST_ID,data_send,8)))
+        return 1;
+    else
+        return 0;
+}
+
+//posila PDO s zadanou hodnotou rychlosti
+int CANbus::send_PDO_speed(int velocity) {
+    char data_send[6];
+    int vel = (int)(velocity * 286331.153);
+    data_send[0] = 0xFF & vel;
+    data_send[1] = (0xFF00 & vel) >> 8; 
+    data_send[2] = (0xFF0000 & vel) >> 16;
+    data_send[3] = (0xFF000000 & vel) >> 24;
+    data_send[4] = 0x00;
+    data_send[5] = 0x00;
+    if(Can.write(CANMessage(PDO_RECEIVE_ID,data_send,6)))
+        return 1;
+    else
+        return 0;
+}
+
+//posila PDO s zadanou hodnotou proudu
+int CANbus::send_PDO_current(float curr) {
+    char data_send[6];
+    int cur = (int)(curr*1434.816);
+    data_send[0] = 0xFF & cur;
+    data_send[1] = (0xFF00 & cur) >> 8; 
+    data_send[2] = 0x00;
+    data_send[3] = 0x00;
+    data_send[4] = 0x00;
+    data_send[5] = 0x00;
+    if(Can.write(CANMessage(PDO_RECEIVE_ID,data_send,6)))
+        return 1;
+    else
+        return 0;
+}
+
+//posila SDO zadost na vycteni rychlosti
+void CANbus::actual_speed() {
+    char data_send[8];
+    data_send[0] = CMD_SDO_READ_REQ;
+    data_send[1] = 0x9A;
+    data_send[2] = 0x01;
+    data_send[3] = 0x2;
+    data_send[4] = 0x0;
+    data_send[5] = 0x0;
+    data_send[6] = 0x0;
+    data_send[7] = 0x0;
+    
+    if(Can.write(CANMessage(SDO_REQUEST_ID ,data_send,8)));
+}
+
+//vysila SDO s zadosti na vycteni modu
+void CANbus::get_mode() {
+    char data_send[8];
+    data_send[0] = CMD_SDO_READ_REQ;
+    data_send[1] = 0xEF;
+    data_send[2] = 0x01;
+    data_send[3] = 0x2;
+    data_send[4] = 0x0;
+    data_send[5] = 0x0;
+    data_send[6] = 0x0;
+    data_send[7] = 0x0;
+    
+    if(Can.write(CANMessage(SDO_REQUEST_ID ,data_send,8)));
+}
+
+//vysila SDO s zadosti o vycteni proudu do motoru
+void CANbus::actual_current() {
+    char data_send[8];
+    data_send[0] = CMD_SDO_READ_REQ;
+    data_send[1] = 0xA8;
+    data_send[2] = 0x01;
+    data_send[3] = 0x1;
+    data_send[4] = 0x0;
+    data_send[5] = 0x0;
+    data_send[6] = 0x0;
+    data_send[7] = 0x0;
+    
+    if(Can.write(CANMessage(SDO_REQUEST_ID ,data_send,8)));
+
+}
+
+//obsluha preruseni CANu
+void CANbus::CAN_frame_received(void) {
+    char curr[2];
+    Can.read(canmsg);
+    switch(canmsg.id) {
+        case SDO_RESPONSE_ID: 
+            cmd = canmsg.data[0];
+            address[0] = canmsg.data[1];
+            address[1] = canmsg.data[2];
+            data_size = canmsg.data[3];
+            data[0] = canmsg.data[4];
+            data[1] = canmsg.data[5];
+            data[2] = canmsg.data[6];
+            data[3] = canmsg.data[7];
+            if((address[0] == 0x9A) && (address[1] == 0x1)){
+                rSpeed = (((float)get_data(data, 0x02)/286331.153)*0.10472)/7.7;
+                //mSpeed = ((float)get_data(data, 0x02)/286331.153);
+                }
+            if((address[0] == 0xEF) && (address[1] == 0x1))
+                mode = get_data(data, 0x02);
+            break;
+            
+        case PDO_TRANSMIT_ID: /*plati pro rychlostni a momentove rizeni*/
+            curr[0] = canmsg.data[5];
+            curr[1] = canmsg.data[6];
+            current = (float)((short int)get_data(curr,1)/1434.816);
+            break;
+        default: 
+            break;
+    }
+}
+
+void CANbus::enable_servo() {
+    send_SDO_request(CMD_SDO_WRITE_REQ_2,MASTER_CMD,1,REQ_ENABLE_SERVO);
+}
+
+void CANbus::disable_servo() {
+    send_SDO_request(CMD_SDO_WRITE_REQ_2,MASTER_CMD,1,REQ_DISABLE_SERVO);
+}
+
+void CANbus::set_mode() {
+    send_SDO_request(CMD_SDO_WRITE_REQ_4,SERVO_MODE,2,MOMENT_MODE);
+}
+
+void CANbus::set_current(float cur) {
+    send_PDO_current(cur);    
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/can.h	Tue Feb 10 14:31:40 2015 +0000
@@ -0,0 +1,60 @@
+#ifndef CANPROTOCOL_H
+#define CANPROTOCOL_H
+
+#define ID_SERVO            0x01
+#define SDO_REQUEST_ID      0x600+ID_SERVO
+#define SDO_RESPONSE_ID     0x580+ID_SERVO
+#define PDO_TRANSMIT_ID     0x180+ID_SERVO
+#define PDO_RECEIVE_ID      0x200+ID_SERVO
+#define SYNC_ID             0x80
+  
+#define SERVO_MODE          0x1EF
+#define MASTER_CMD          0x1ED
+
+
+#define CMD_SDO_READ_REQ        0x40
+#define CMD_SDO_READ_RES_2      0x4B
+#define CMD_SDO_READ_RES_4      0x43
+
+#define CMD_SDO_WRITE_REQ_24    0x22
+#define CMD_SDO_WRITE_REQ_2     0x2B         
+#define CMD_SDO_WRITE_REQ_4     0x23
+#define CMD_SDO_WRITE_RES       0x60
+#define CMD_SDO_WRITE_ERR       0x80
+
+
+#define REQ_DISABLE_SERVO       0x01
+#define REQ_ENABLE_SERVO        0x02
+
+#define SPEED_MODE              0x200002
+#define MOMENT_MODE             0x200001
+
+extern float current;
+extern int vel;
+extern int mode;
+extern float rSpeed;
+extern float mSpeed;
+extern char data[4],address[2],data_size,cmd;
+
+class CANbus {
+    CAN Can;
+    int get_data(char data[], char size);
+    int send_SDO_request(char cmd, int adr, char size,int data);
+    int send_PDO_speed(int velocity);
+    int send_PDO_current(float curr);
+    
+public:
+    CANbus(PinName CANpin1,PinName CANpin2);
+    void CAN_frame_received(void);
+    void servo_sync(void);
+    void enable_servo(void);
+    void disable_servo(void);
+    void set_mode(void);
+    void get_mode(void);
+    void set_current(float cur);
+    void actual_speed(void); 
+    void actual_current(void);
+};
+
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Feb 10 14:31:40 2015 +0000
@@ -0,0 +1,74 @@
+#include "mbed.h"
+#include "rtos.h"
+#include "pin.h"
+#include "angle.h"
+#include "can.h"
+
+Serial pc(USBTX, USBRX);
+IRsensor sharp(p17);
+CANbus can(p30,p29);
+
+DigitalOut led(LED1);
+//CANbus can(p30,p29);
+
+Mutex mutex1;
+
+float curr_req = 0;
+float K[3] = {17.0, 3.6, -0.14};
+float Mz = 0;
+float Mm = 0;
+extern float vel_temp = 286331.153;
+float phi;      //uhel micku na kole
+float omega;    //uhlova rychlost micku
+float phi_old;  //starsi hodnota uhlu micku
+
+int jj = 0;
+
+
+//vlakno pro snimani uhlu micku
+void sensor_thread(void const *args) {
+    while(true) {
+        mutex1.lock();
+        phi = sharp.get_angle();
+        omega = (phi - phi_old)/0.040;
+        phi_old = phi;
+        //pc.printf("%2.3f\n",phi);
+        mutex1.unlock();
+        Thread::wait(40);
+    }
+}
+
+//vlakno pro vysilani synchronizace
+void sync_thread(void const *args) {
+    while(true) {
+        can.servo_sync();
+        Thread::wait(5);
+    }
+}
+
+
+    
+int main() {
+    pc.baud(19200);
+    pc.format(8,SerialBase::None,1);
+
+    Thread thread_sens(sensor_thread);
+    Thread thread_sync(sync_thread);
+    i = 20;
+    //can.can_init();
+    can.enable_servo();
+    can.set_mode();
+    can.set_current(0);
+    
+    while(phi < 0.3 && (rSpeed < 25 || rSpeed > -25) && (curr_req < 15.0 || curr_req > -15.0)){
+        can.actual_speed();
+        Mz = (phi*K[0] +omega*K[1] + rSpeed*K[2]);
+        Mm = Mz/7.7;
+        curr_req = Mm/0.3;
+        printf("rSpeed = %2.2f phi = %2.2f comega= %2.2f Mz=%2.2f Mm = %2.2f curr = %2.2f req_curr = %2.2f\n",rSpeed, phi,omega, Mz, Mm,current,curr_req);
+        can.set_current(curr_req);
+        //printf("phi = %2.2f\n",phi);
+        wait(0.04);
+    }
+    can.disable_servo();
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-rtos.lib	Tue Feb 10 14:31:40 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed-rtos/#5448826aa700

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue Feb 10 14:31:40 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/4fc01daae5a5
\ No newline at end of file