/* Writes the given packet out on both channels
*
* returns: 0 on success, negative error code otherwise
*/
int
CANIOKvaser::WritePacket(CanPacket &pkt)
{
int ret;
//printf("CANIO: WRITE: pkt: %s\n", pkt.toString());
for (int i=0; i < DUALCAN_NR_CHANNELS; i++) {
if ((ret = canWriteWait(channels[i], pkt.id, pkt.msg, pkt.dlc,
pkt.flags, 1000)) < 0) {
printf("CANIO: write wait error %d\n", ret);
return ret;
}
if ((ret = canWriteSync(channels[i], 10000)) < 0) {
printf("CANIO: error %d on write sync\n", ret);
switch (ret) {
case canERR_TIMEOUT:
printf("CANIO: TIMEOUT error\n");
break;
default:
break;
}
return ret;
}
}
return 0;
}
void enable_torque_assist() {
// Unit responds to input torque, provides output torque according to
// programmed assist curve.
char msg_buf[]={0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
msg_buf[0]=0x02;
check("canWrite", canWrite(h, CAN_ID, (void*)msg_buf, 8, canMSG_EXT));
check("canWriteSync", canWriteSync(h, 1000));
}
int main (int argc, char *argv[])
{
canHandle h;
int channel;
int bitrate = BAUD_1M;
errno = 0;
if (argc != 3 || (channel = 0, errno) != 0) {
printf("usage %s address byte1\n", argv[0]);
exit(1);
} else {
printf("Sending a message on channel %d\n", channel);
}
/* Allow signals to interrupt syscalls(e.g in canReadBlock) */
siginterrupt(SIGINT, 1);
/* Open channel, set parameters and go on bus */
//h = canOpenChannel(channel, canOPEN_EXCLUSIVE | canOPEN_REQUIRE_EXTENDED);
h = canOpenChannel(channel, canOPEN_EXCLUSIVE);
if (h < 0) {
printf("canOpenChannel %d failed\n", channel);
return -1;
}
canBusOff(h);
check("canSetBusParams", canSetBusParams(h, bitrate, 4, 3, 2, 1, 0));
// Work-around for Leaf bug
check("canSetBusOutputControl", canSetBusOutputControl(h, canDRIVER_NORMAL));
check("canBusOn", canBusOn(h));
//OY 8/20/2014
//unsigned char channel = atoi(argv[2]));
msg[0] = 0;
msg[2] = 1;
msg[3] = 41;
if (atoi(argv[2]) == 0)
msg[1] = 1;
if (atoi(argv[2]) == 100)
msg[1] = 2;
check("canWrite", canWrite(h, atoi(argv[1]), msg, 4, 0));
//printf("%d %d %d %d %d\n",msg[0], msg[1], msg[2], msg[3], channel);
check("canWriteSync", canWriteSync(h, 1000));
//check("canBusOff", canBusOff(h));
check("canClose", canClose(h));
return 0;
}
void motor_off() {
// Turn off the motor's drive to stop the motor. Can still be backdriven.
char msg_buf[]={0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
msg_buf[0]=0x00;
msg_buf[2]=0x00;
msg_buf[3]=0x00;
msg_buf[4]=0x00;
msg_buf[5]=0x00;
check("canWrite", canWrite(h, CAN_ID, (void*)msg_buf, 8, canMSG_EXT));
check("canWriteSync", canWriteSync(h, 1000));
}
void set_position(double angular_position) {
// Position control mode for the Globe EPAS motor. Specified current in degrees.
char msg_buf[]={0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
int Q20_angular_position = (int)(angular_position/360.0*pow(2,20));
msg_buf[0]=0x04;
msg_buf[2]=(Q20_angular_position&0x000000FF);
msg_buf[3]=(Q20_angular_position&0x0000FF00)>>8;
msg_buf[4]=(Q20_angular_position&0x00FF0000)>>16;
msg_buf[5]=(Q20_angular_position&0xFF000000)>>24;
check("canWrite", canWrite(h, CAN_ID, (void*)msg_buf, 8, canMSG_EXT));
check("canWriteSync", canWriteSync(h, 1000));
}
void set_speed(double angular_speed) {
// Speed control mode for the Globe EPAS motor. Specified speed in
// degrees/second, internally limited to +/- 20 rev/second.
char msg_buf[]={0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
int Q20_angular_speed = (int)(angular_speed/360.0*gear_ratio*pow(2,20));
msg_buf[0]=0x03;
msg_buf[2]=(Q20_angular_speed&0x000000FF);
msg_buf[3]=(Q20_angular_speed&0x0000FF00)>>8;
msg_buf[4]=(Q20_angular_speed&0x00FF0000)>>16;
msg_buf[5]=(Q20_angular_speed&0xFF000000)>>24;
check("canWrite", canWrite(h, CAN_ID, (void *) msg_buf, 8, canMSG_EXT));
check("canWriteSync", canWriteSync(h, 1000));
}
void set_current(double current) {
// Current control mode for the Globe EPAS motor. Specified current in amps,
// internally limited to +/- 60A.
char msg_buf[]={0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
int Q20_current = (int)(current*pow(2,20));
msg_buf[0]=0x01;
msg_buf[2]=(Q20_current&0x000000FF);
msg_buf[3]=(Q20_current&0x0000FF00)>>8;
msg_buf[4]=(Q20_current&0x00FF0000)>>16;
msg_buf[5]=(Q20_current&0xFF000000)>>24;
check("canWrite", canWrite(h, CAN_ID, (void*)msg_buf, 8, canMSG_EXT));
check("canWriteSync", canWriteSync(h, 1000));
}
void set_inc_encoder_value(double angular_position) {
// Sets the value of the internal encoder to the specified position (in
// degrees). Doesn't move the motor.
char msg_buf[]={0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
int Q20_angular_position = (int)(angular_position/360.0*pow(2,20));
msg_buf[0]=0x10;
msg_buf[2]=(Q20_angular_position&0x000000FF);
msg_buf[3]=(Q20_angular_position&0x0000FF00)>>8;
msg_buf[4]=(Q20_angular_position&0x00FF0000)>>16;
msg_buf[5]=(Q20_angular_position&0xFF000000)>>24;
check("canWrite", canWrite(h, CAN_ID, (void*)msg_buf, 8, canMSG_EXT));
check("canWriteSync", canWriteSync(h, 1000));
}
void set_position_with_speed_limit(double angular_position, double angular_speed) {
// Position control mode for the Globe EPAS motor, with speed limit.
// Specified position in degrees, speed in degrees/second.
char msg_buf[]={0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
int Q20_angular_position = (int)(angular_position/360.0*pow(2,20));
int Q8_angular_speed = (int)(angular_speed/360.0*pow(2,8));
msg_buf[0]=0x05;
msg_buf[2]=(Q20_angular_position&0x000000FF);
msg_buf[3]=(Q20_angular_position&0x0000FF00)>>8;
msg_buf[4]=(Q20_angular_position&0x00FF0000)>>16;
msg_buf[5]=(Q20_angular_position&0xFF000000)>>24;
msg_buf[6]=(Q8_angular_speed&0x00FF);
msg_buf[7]=(Q8_angular_speed&0xFF00)>>8;
check("canWrite", canWrite(h, CAN_ID, (void*)msg_buf, 8, canMSG_EXT));
check("canWriteSync", canWriteSync(h, 1000));
}
