Roman Krejci / Mbed 2 deprecated DP_28_4_2015

DP

Dependencies:   FastAnalogIn mbed-rtos mbed

Fork of dipl_prace_v10 by Roman Krejci

can.cpp@1:28d74f044818, 2015-04-28 (annotated)

Committer:
romankrej
Date:
Tue Apr 28 18:48:50 2015 +0000
Revision:
1:28d74f044818
Parent:
0:f3b355df6f26 
DP

Who changed what in which revision?

UserRevisionLine numberNew contents of line
romankrej 0:f3b355df6f26 1 #include "can.h"
romankrej 0:f3b355df6f26 2 #include "threads.h"
romankrej 0:f3b355df6f26 3
romankrej 0:f3b355df6f26 4 CANMessage canmsg;
romankrej 0:f3b355df6f26 5
romankrej 0:f3b355df6f26 6 /*
romankrej 0:f3b355df6f26 7 * Constructor
romankrej 0:f3b355df6f26 8 */
romankrej 0:f3b355df6f26 9 cCan::cCan(PinName CANpin1, PinName CANpin2) : Can(CANpin1,CANpin2) {
romankrej 0:f3b355df6f26 10
romankrej 0:f3b355df6f26 11 Can.frequency(CAN_FREQ);
romankrej 0:f3b355df6f26 12 Can.attach(this,&cCan::CAN_frame_received);
romankrej 0:f3b355df6f26 13 cCan::setMode(MOMENT_MODE);
romankrej 0:f3b355df6f26 14 cCan::enableServo();
romankrej 0:f3b355df6f26 15 cCan::nullResolver();
romankrej 0:f3b355df6f26 16 }
romankrej 0:f3b355df6f26 17
romankrej 0:f3b355df6f26 18
romankrej 0:f3b355df6f26 19 /*
romankrej 0:f3b355df6f26 20 * This method sends synchronization frame
romankrej 0:f3b355df6f26 21 */
romankrej 0:f3b355df6f26 22 void cCan::sync(void) {
romankrej 0:f3b355df6f26 23 Can.write(CANMessage(0x80));
romankrej 0:f3b355df6f26 24 }
romankrej 0:f3b355df6f26 25
romankrej 0:f3b355df6f26 26
romankrej 0:f3b355df6f26 27 /*
romankrej 0:f3b355df6f26 28 * This method convert bytes to data
romankrej 0:f3b355df6f26 29 */
romankrej 0:f3b355df6f26 30 int cCan::get_data(char data[], char size) {
romankrej 0:f3b355df6f26 31 int ret_data = 0;
romankrej 0:f3b355df6f26 32 for(int i = 0; i < 2*size; i++) {
romankrej 0:f3b355df6f26 33 ret_data |= data[i] << 8*i;
romankrej 0:f3b355df6f26 34 }
romankrej 0:f3b355df6f26 35 return ret_data;
romankrej 0:f3b355df6f26 36 }
romankrej 0:f3b355df6f26 37
romankrej 0:f3b355df6f26 38 /*
romankrej 0:f3b355df6f26 39 * This method seds SDO request to servoamplifier
romankrej 0:f3b355df6f26 40 */
romankrej 0:f3b355df6f26 41 int cCan::send_SDO_request(char cmd, int adr, char size,int data) {
romankrej 0:f3b355df6f26 42 char data_send[8];
romankrej 0:f3b355df6f26 43 data_send[0] = cmd;
romankrej 0:f3b355df6f26 44 data_send[1] = 0xFF & adr;;
romankrej 0:f3b355df6f26 45 data_send[2] = (0xFF00 & adr) >> 8;
romankrej 0:f3b355df6f26 46 data_send[3] = size;
romankrej 0:f3b355df6f26 47 data_send[4] = 0xFF & data;
romankrej 0:f3b355df6f26 48 data_send[5] = (0xFF00 & data) >> 8;
romankrej 0:f3b355df6f26 49 data_send[6] = (0xFF0000 & data)>>16;
romankrej 0:f3b355df6f26 50 data_send[7] = (0xFF000000 & data)>>24;
romankrej 0:f3b355df6f26 51 if(Can.write(CANMessage(SDO_REQUEST_ID,data_send,8)))
romankrej 0:f3b355df6f26 52 return 1;
romankrej 0:f3b355df6f26 53 else
romankrej 0:f3b355df6f26 54 return 0;
romankrej 0:f3b355df6f26 55 }
romankrej 0:f3b355df6f26 56
romankrej 0:f3b355df6f26 57
romankrej 0:f3b355df6f26 58 /*
romankrej 0:f3b355df6f26 59 * This method sends PDO with required current
romankrej 0:f3b355df6f26 60 */
romankrej 0:f3b355df6f26 61 int cCan::send_PDO_current(float curr) {
romankrej 0:f3b355df6f26 62 char data_send[6];
romankrej 0:f3b355df6f26 63 int cur = (int)(-curr*1434.816);
romankrej 0:f3b355df6f26 64 data_send[0] = 0xFF & cur;
romankrej 0:f3b355df6f26 65 data_send[1] = (0xFF00 & cur) >> 8;
romankrej 0:f3b355df6f26 66 data_send[2] = 0x00;
romankrej 0:f3b355df6f26 67 data_send[3] = 0x00;
romankrej 0:f3b355df6f26 68 data_send[4] = 0x00;
romankrej 0:f3b355df6f26 69 data_send[5] = 0x00;
romankrej 0:f3b355df6f26 70 if(Can.write(CANMessage(PDO_RECEIVE_ID,data_send,6))) {
romankrej 0:f3b355df6f26 71 return 1;
romankrej 0:f3b355df6f26 72 }
romankrej 0:f3b355df6f26 73 else {
romankrej 0:f3b355df6f26 74
romankrej 0:f3b355df6f26 75 return 0;
romankrej 0:f3b355df6f26 76 }
romankrej 0:f3b355df6f26 77 }
romankrej 0:f3b355df6f26 78
romankrej 0:f3b355df6f26 79 /*
romankrej 0:f3b355df6f26 80 * This method sends SDO request of actual speed
romankrej 0:f3b355df6f26 81 */
romankrej 0:f3b355df6f26 82 void cCan::actual_speed() {
romankrej 0:f3b355df6f26 83 char data_send[8];
romankrej 0:f3b355df6f26 84 data_send[0] = CMD_SDO_READ_REQ;
romankrej 0:f3b355df6f26 85 data_send[1] = 0x9A;
romankrej 0:f3b355df6f26 86 data_send[2] = 0x01;
romankrej 0:f3b355df6f26 87 data_send[3] = 0x2;
romankrej 0:f3b355df6f26 88 data_send[4] = 0x0;
romankrej 0:f3b355df6f26 89 data_send[5] = 0x0;
romankrej 0:f3b355df6f26 90 data_send[6] = 0x0;
romankrej 0:f3b355df6f26 91 data_send[7] = 0x0;
romankrej 0:f3b355df6f26 92
romankrej 0:f3b355df6f26 93 if(Can.write(CANMessage(SDO_REQUEST_ID ,data_send,8)));
romankrej 0:f3b355df6f26 94 }
romankrej 0:f3b355df6f26 95
romankrej 0:f3b355df6f26 96 /*
romankrej 0:f3b355df6f26 97 * This method send SDO request of mode of servo
romankrej 0:f3b355df6f26 98 */
romankrej 0:f3b355df6f26 99 void cCan::get_mode() {
romankrej 0:f3b355df6f26 100 char data_send[8];
romankrej 0:f3b355df6f26 101 data_send[0] = CMD_SDO_READ_REQ;
romankrej 0:f3b355df6f26 102 data_send[1] = 0xEF;
romankrej 0:f3b355df6f26 103 data_send[2] = 0x01;
romankrej 0:f3b355df6f26 104 data_send[3] = 0x2;
romankrej 0:f3b355df6f26 105 data_send[4] = 0x0;
romankrej 0:f3b355df6f26 106 data_send[5] = 0x0;
romankrej 0:f3b355df6f26 107 data_send[6] = 0x0;
romankrej 0:f3b355df6f26 108 data_send[7] = 0x0;
romankrej 0:f3b355df6f26 109
romankrej 0:f3b355df6f26 110 if(Can.write(CANMessage(SDO_REQUEST_ID ,data_send,8)));
romankrej 0:f3b355df6f26 111 }
romankrej 0:f3b355df6f26 112
romankrej 0:f3b355df6f26 113 /*
romankrej 0:f3b355df6f26 114 * This method is function when CAN frame received
romankrej 0:f3b355df6f26 115 */
romankrej 0:f3b355df6f26 116 void cCan::CAN_frame_received(void) {
romankrej 0:f3b355df6f26 117 char curr[2];
romankrej 0:f3b355df6f26 118 char pos[4] = {0,0,0,0};
romankrej 0:f3b355df6f26 119 Can.read(canmsg);
romankrej 0:f3b355df6f26 120 switch(canmsg.id) {
romankrej 0:f3b355df6f26 121
romankrej 0:f3b355df6f26 122 case SDO_RESPONSE_ID:
romankrej 0:f3b355df6f26 123 cmd = canmsg.data[0];
romankrej 0:f3b355df6f26 124 address[0] = canmsg.data[1];
romankrej 0:f3b355df6f26 125 address[1] = canmsg.data[2];
romankrej 0:f3b355df6f26 126 data_size = canmsg.data[3];
romankrej 0:f3b355df6f26 127 data[0] = canmsg.data[4];
romankrej 0:f3b355df6f26 128 data[1] = canmsg.data[5];
romankrej 0:f3b355df6f26 129 data[2] = canmsg.data[6];
romankrej 0:f3b355df6f26 130 data[3] = canmsg.data[7];
romankrej 0:f3b355df6f26 131
romankrej 0:f3b355df6f26 132 if((address[0] == 0x9A) && (address[1] == 0x1)){
romankrej 0:f3b355df6f26 133 omega = (((float)get_data(data, 0x02)/286331.153)*-0.10472)/7.7;
romankrej 0:f3b355df6f26 134 thread->signal_set(0x03);
romankrej 0:f3b355df6f26 135 }
romankrej 0:f3b355df6f26 136
romankrej 0:f3b355df6f26 137 if((address[0] == 0xEF) && (address[1] == 0x1))
romankrej 0:f3b355df6f26 138 mode = get_data(data, 0x02);
romankrej 0:f3b355df6f26 139 break;
romankrej 0:f3b355df6f26 140
romankrej 0:f3b355df6f26 141 case PDO_TRANSMIT_ID:
romankrej 0:f3b355df6f26 142
romankrej 0:f3b355df6f26 143 pos[0] = canmsg.data[1];
romankrej 0:f3b355df6f26 144 pos[1] = canmsg.data[2];
romankrej 0:f3b355df6f26 145 pos[2] = canmsg.data[3];
romankrej 0:f3b355df6f26 146 pos[3] = canmsg.data[4];
romankrej 0:f3b355df6f26 147 curr[0] = canmsg.data[5];
romankrej 0:f3b355df6f26 148 curr[1] = canmsg.data[6];
romankrej 0:f3b355df6f26 149
romankrej 0:f3b355df6f26 150 current = (float)(short int)get_data(curr,1)*(-0.000697);
romankrej 0:f3b355df6f26 151 phi = (float)(get_data(pos,2))*(-0.000012448);
romankrej 0:f3b355df6f26 152 thread->signal_set(0x02);
romankrej 0:f3b355df6f26 153 break;
romankrej 0:f3b355df6f26 154
romankrej 0:f3b355df6f26 155 default:
romankrej 0:f3b355df6f26 156 break;
romankrej 0:f3b355df6f26 157 }
romankrej 0:f3b355df6f26 158 }
romankrej 0:f3b355df6f26 159
romankrej 0:f3b355df6f26 160 /*
romankrej 0:f3b355df6f26 161 * For enable servo
romankrej 0:f3b355df6f26 162 */
romankrej 0:f3b355df6f26 163 void cCan::enableServo() {
romankrej 0:f3b355df6f26 164 send_SDO_request(CMD_SDO_WRITE_REQ_2,MASTER_CMD,1,REQ_ENABLE_SERVO);
romankrej 0:f3b355df6f26 165 }
romankrej 0:f3b355df6f26 166
romankrej 0:f3b355df6f26 167 /*
romankrej 0:f3b355df6f26 168 * For disable servo
romankrej 0:f3b355df6f26 169 */
romankrej 0:f3b355df6f26 170 void cCan::disableServo() {
romankrej 0:f3b355df6f26 171 send_SDO_request(CMD_SDO_WRITE_REQ_2,MASTER_CMD,1,REQ_DISABLE_SERVO);
romankrej 0:f3b355df6f26 172 }
romankrej 0:f3b355df6f26 173
romankrej 0:f3b355df6f26 174 /*
romankrej 0:f3b355df6f26 175 * For null resolver
romankrej 0:f3b355df6f26 176 */
romankrej 0:f3b355df6f26 177 void cCan::nullResolver() {
romankrej 0:f3b355df6f26 178 send_SDO_request(CMD_SDO_WRITE_REQ_2,MASTER_CMD,1,REQ_NULL_RES);
romankrej 0:f3b355df6f26 179 }
romankrej 0:f3b355df6f26 180
romankrej 0:f3b355df6f26 181 /*
romankrej 0:f3b355df6f26 182 * For set mode of servo
romankrej 0:f3b355df6f26 183 */
romankrej 0:f3b355df6f26 184 void cCan::setMode(int req_mode) {
romankrej 0:f3b355df6f26 185 send_SDO_request(CMD_SDO_WRITE_REQ_4,SERVO_MODE,2,req_mode);
romankrej 0:f3b355df6f26 186 }
romankrej 0:f3b355df6f26 187
romankrej 0:f3b355df6f26 188 /*
romankrej 0:f3b355df6f26 189 * For setting required current
romankrej 0:f3b355df6f26 190 */
romankrej 0:f3b355df6f26 191 void cCan::setCurrent(float cur) {
romankrej 0:f3b355df6f26 192 send_PDO_current(cur);
romankrej 0:f3b355df6f26 193 }
romankrej 0:f3b355df6f26 194
romankrej 0:f3b355df6f26 195 /*
romankrej 0:f3b355df6f26 196 *Get angular position of wheel
romankrej 0:f3b355df6f26 197 */
romankrej 0:f3b355df6f26 198 float cCan::getPhi() {
romankrej 0:f3b355df6f26 199 return phi;
romankrej 0:f3b355df6f26 200 }
romankrej 0:f3b355df6f26 201
romankrej 0:f3b355df6f26 202 /*
romankrej 0:f3b355df6f26 203 * Get angular velocity of wheel
romankrej 0:f3b355df6f26 204 */
romankrej 0:f3b355df6f26 205 float cCan::getOmega() {
romankrej 0:f3b355df6f26 206 return omega;
romankrej 0:f3b355df6f26 207 }