unsigned int baud_time_get()
{
#ifdef ___MAPLE
timer4.pause();
unsigned int tmp = timer4.getCount(); //get current timer count
timer4.setCount(0); // reset timer count
timer4.refresh();
timer4.resume();
#endif
#ifdef ___ARDUINO
TCCR1B &= 0xF8; // Clear CS10/CS11/CS12 bits, stopping the timer
unsigned int tmp = int(TCNT1/2); // get current timer count, dividing by two
// because Arduino timer is counting at 2Mhz instead of 1Mhz
// due to limited prescaler selection
tmp += 0x8000 * baudOverflow; // if timer overflowed, we will add 0xFFFF/2
TCNT1 = 0; // reset timer count
baudOverflow = false; // reset baudOverflow after resetting timer
TCCR1B |= (1 << CS11); // Configure for 8 prescaler, restarting timer
#endif
#ifdef ___TEENSY
unsigned int tmp = int(FTM0_CNT*2.8);
FTM0_CNT = 0x0;
#endif
return tmp;
}
void motor_read_current() {
if (HAS_CURRENT_SENSING) {
mot.current = analogRead(CURRENT_ADC_PIN) - 2048;
if (abs(mot.current) > 500) {
// Values that big are not taken into account. This is a bad hack and can
// be optimized.
} else {
mot.averageCurrent =
((AVERAGE_FACTOR_FOR_CURRENT - 1) * mot.averageCurrent * PRESCALE +
mot.current * PRESCALE) /
(AVERAGE_FACTOR_FOR_CURRENT * PRESCALE);
}
if (currentDetailedDebugOn == true) {
currentRawMeasures[currentMeasureIndex] = mot.current;
currentTimming[currentMeasureIndex] = timer3.getCount();
currentMeasureIndex++;
if (currentMeasureIndex > (C_NB_RAW_MEASURES - 1)) {
currentDetailedDebugOn = false;
currentMeasureIndex = 0;
}
}
}
}
static void serial_received(struct serial *serial)
{
dma_disable(DMA1, serial->channel);
usart_tcie(serial->port->c_dev()->regs, 0);
serial->dmaEvent = false;
serial->txComplete = true;
receiveMode(serial);
serial->syncReadStart = syncReadTimer.getCount();
}
void motor_add_benchmark_time() {
if (timeIndexMotor < TIME_STAMP_SIZE_MOTOR) {
arrayOfTimeStampsMotor[timeIndexMotor] = timer3.getCount();
timeIndexMotor++;
}
}
void motor_update(encoder *pEnc) {
uint16 time = timer3.getCount();
int16 dt = 10; /*
* This function should be called every 1 ms.
* The plan B would be to estimate dt = time - previousTime;
* */
// int16 dt = time - previousTime; // Need to check for the case where the
// timer overflows or resets.
// previousTime = time;
// Updating the motor position (ie angle)
buffer_add(&(mot.angleBuffer), mot.angle);
// This command is the one from the previous tick since the control is updated
// after the motor.
// buffer_add(&(mot.commandBuffer), mot.command);
mot.previousAngle = mot.angle;
// Taking into account the magic offset
int tempAngle = pEnc->angle + mot.offset;
if (tempAngle < 0) {
tempAngle = MAX_ANGLE + tempAngle + 1;
}
// Should not be useful, just being careful
tempAngle = tempAngle % (MAX_ANGLE + 1);
mot.angle = tempAngle;
if (mot.multiTurnOn == false) {
mot.multiTurnAngle = mot.angle;
} else {
if ((mot.angle - mot.previousAngle) > (MAX_ANGLE + 1) / 2) {
// Went from 3 to 4092 (4092 - 3 > 2048), the multiturn angle should be
// what it was minus 7
mot.multiTurnAngle =
mot.multiTurnAngle + mot.angle - mot.previousAngle - (MAX_ANGLE + 1);
} else if ((mot.angle - mot.previousAngle) < (-MAX_ANGLE - 1) / 2) {
// Went from 4095 to 1, the multiturn angle should be what it was plus 2
mot.multiTurnAngle =
mot.multiTurnAngle + mot.angle - mot.previousAngle + (MAX_ANGLE + 1);
} else {
mot.multiTurnAngle = mot.multiTurnAngle + mot.angle - mot.previousAngle;
}
}
int16 oldPosition = buffer_get(&(mot.angleBuffer));
motor_update_sign_of_speed();
// Updating the motor speed
int32 previousSpeed = mot.speed;
//mot.speed = mot.angle - oldPosition;
// New way of calculating the speed
mot.speed = filter_speed_update(&mot.filt_speed, mot.angle);
if (abs(mot.speed) > MAX_ANGLE / 2) {
// Position went from near max to near 0 or vice-versa
if (mot.angle >= oldPosition) {
mot.speed = mot.speed - MAX_ANGLE - 1;
} else if (mot.angle < oldPosition) {
mot.speed = mot.speed + MAX_ANGLE + 1;
}
}
// The speed will be in steps/s :
mot.speed = (mot.speed * 1000) / dxl_regs.ram.speedCalculationDelay;
// Averaging with previous value (dangerous approach):
mot.averageSpeed = ((previousSpeed * 99) + (mot.speed)) / 100.0;
buffer_add(&(mot.speedBuffer), mot.speed);
// Updating the motor acceleration
int32 oldSpeed = buffer_get(&(mot.speedBuffer));
mot.acceleration = mot.speed - oldSpeed;
if (predictiveCommandOn) {
if (changeId == 0) {
changeId = timeIndexMotor;
}
if (counterUpdate % (dxl_regs.ram.predictiveCommandPeriod) == 0) {
if (time > dxl_regs.ram.duration1 && controlMode != COMPLIANT_KIND_OF) {
motor_restart_traj_timer();
time = 0;
if (dxl_regs.ram.copyNextBuffer != 0) {
// Copying the buffer into the actual trajs
dxl_regs.ram.copyNextBuffer = 0;
dxl_copy_buffer_trajs();
} else {
digitalWrite(BOARD_LED_PIN, LOW);
// Default action : forcing the motor to stay where it stands (through
// PID)
controlMode = POSITION_CONTROL;
dxl_regs.ram.mode = 0;
hardwareStruct.mot->targetAngle = hardwareStruct.mot->angle;
dxl_regs.ram.goalPosition = hardwareStruct.mot->targetAngle;
}
}
// These 3 lines make it impossible for the bootloader to load the binary
// file, mainly because of the cos import. -> Known bug and known solution
// but quite time consuming.
// float angleRad = (mot.angle * (float)PI) / 2048.0;
// float weightCompensation = cos(angleRad) * 71;
// predictive_control_anti_gravity_tick(&mot, mot.speed,
// weightCompensation, addedInertia);
// We're going to evaluate at least one polynom (and more often than not,
// 3 polynoms). We'll calculate the powers of t only once :
int maxPower =
max(dxl_regs.ram.trajPoly1Size, dxl_regs.ram.torquePoly1Size);
float timePowers[4];
eval_powers_of_t(timePowers, time, maxPower, 10000);
if (controlMode == COMPLIANT_KIND_OF) {
predictive_control_anti_gravity_tick(&mot, mot.speed, 0.0, 0.0);
} else {
predictive_control_tick(
&mot,
(int32)traj_eval_poly_derivate(dxl_regs.ram.trajPoly1, timePowers),
dt * (dxl_regs.ram.predictiveCommandPeriod),
traj_eval_poly(dxl_regs.ram.torquePoly1, timePowers), 0);
}
if (controlMode == PID_AND_PREDICTIVE_COMMAND ||
controlMode == PID_ONLY) {
mot.targetAngle = traj_magic_modulo(
round(traj_eval_poly(dxl_regs.ram.trajPoly1, timePowers)),
MAX_ANGLE + 1);
}
if (dxl_regs.ram.positionTrackerOn == true) {
// For arbitrary measures
if (counterUpdate % trackingDivider == 0) {
positionArray[positionIndex] =
mot.angle; // mot.targetAngle;//(int16)weightCompensation;//mot.speed;//mot.predictiveCommand;//traj_min_jerk(timer3.getCount());
timeArray[positionIndex] = time;
if (positionIndex == NB_POSITIONS_SAVED) {
notPrintedYet = false;
positionIndex = 0;
counterUpdate = 0;
dxl_regs.ram.positionTrackerOn = false;
print_detailed_trajectory();
} else {
positionIndex++;
}
}
}
// The estimation of the outputed torque has already been done and sent to
// the ram, updating the motor struct
mot.electricalTorque = dxl_regs.ram.electricalTorque;
mot.outputTorque = dxl_regs.ram.ouputTorque;
}
counterUpdate++;
} else {
if (dxl_regs.ram.positionTrackerOn == true) {
// For arbitrary measures
if (counterUpdate % trackingDivider == 0) {
timeArray[positionIndex] = time;
commandArray[positionIndex] = mot.command;
positionArray[positionIndex] = mot.angle;
speedArray[positionIndex] = mot.speed;
if (positionIndex == NB_POSITIONS_SAVED) {
positionIndex = 0;
counterUpdate = 0;
dxl_regs.ram.positionTrackerOn = false;
print_detailed_trajectory();
} else {
positionIndex++;
}
}
}
counterUpdate++;
// Asking for an estimation of the outputed torque
predictive_update_output_torques(mot.command, mot.speed);
mot.electricalTorque = dxl_regs.ram.electricalTorque;
mot.outputTorque = dxl_regs.ram.ouputTorque;
}
}
/**
* Ticking
*/
static void dxl_serial_tick(volatile struct dxl_device *self)
{
struct serial *serial = (struct serial*)self->data;
static int baudrate = DXL_DEFAULT_BAUDRATE;
// Timeout on sending packet, this should never happen
if (!serial->txComplete && ((millis() - serial->packetSent) > 3)) {
serial_received(serial);
}
/*
if (!serial->txComplete) {
if (serial->dmaEvent) {
// DMA completed
serial->dmaEvent = false;
serial->txComplete = true;
//serial->port->waitDataToBeSent();
receiveMode(serial);
serial->syncReadStart = syncReadTimer.getCount();
}
}
*/
if (serial->txComplete) {
if (syncReadMode) {
bool processed = false;
if (syncReadDevices <= 0 && syncReadResponse == &self->packet) {
// The process is over, no devices are currently trying to get packet and we
// are the device that will respond
syncReadResponse->process = true;
syncReadMode = false;
} else if (serial->syncReadCount) {
if (serial->syncReadCurrent == 0xff) {
// Sending the first packet
serial->syncReadCurrent = 0;
syncReadSendPacket(serial);
} else {
// Reading available data from the port
while (serial->port->available() && !serial->syncReadPacket.process) {
dxl_packet_push_byte(&serial->syncReadPacket, serial->port->read());
}
if (serial->syncReadPacket.process && serial->syncReadPacket.parameter_nb == syncReadLength) {
// The packet is OK, copying data to the response at correct offset
ui8 i;
ui8 off = serial->syncReadOffsets[serial->syncReadCurrent]*(syncReadLength+1);
syncReadResponse->parameters[off] = serial->syncReadPacket.error;
for (i=0; i<syncReadLength; i++) {
syncReadResponse->parameters[off+1+i] = serial->syncReadPacket.parameters[i];
}
processed = true;
} else if (syncReadTimer.getCount()-serial->syncReadStart > 65) {
// The timeout is reached, answer with code 0xff
ui8 off = serial->syncReadOffsets[serial->syncReadCurrent]*(syncReadLength+1);
syncReadResponse->parameters[off] = 0xff;
processed = true;
}
if (processed) {
serial->syncReadCurrent++;
if (serial->syncReadCurrent >= serial->syncReadCount) {
// The process is over for this bus
syncReadDevices--;
serial->syncReadCount = 0;
} else {
// Sending the next packet
syncReadSendPacket(serial);
}
}
}
}
} else {
if (self->redirect_packets == NULL && baudrate != usb_cdcacm_get_baud()) {
// baudrate = usb_cdcacm_get_baud();
// initSerial(serial, baudrate);
}
// Reading data that come from the serial bus
while (serial->port->available() && !self->packet.process) {
dxl_packet_push_byte(&self->packet, serial->port->read());
if (self->packet.process) {
// A packet is coming from our bus, noting it in the devices allocation
// table
devicePorts[self->packet.id] = serial->index;
}
}
}
}
}
