DP
Dependencies: FastAnalogIn mbed-rtos mbed
Fork of dipl_prace_v10 by
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?
User | Revision | Line number | New 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 | } |