void setup_timer_int() {
// Pause the timer while we're configuring it
timer.pause();
// Set up period
timer.setPeriod(TIMER_PERIODE); // in microseconds
// Set up an interrupt on channel 1
timer.setChannel1Mode(TIMER_OUTPUT_COMPARE);
timer.setCompare(TIMER_CH1, 1); // Interrupt 1 count after each update
timer.attachCompare1Interrupt(timer_int_handler);
// Refresh the timer's count, prescale, and overflow
timer.refresh();
// Start the timer counting
timer.resume();
}
void IRAM_ATTR StepperTimerInt() {
hardwareTimer.initializeUs(deltat, StepperTimerInt);
hardwareTimer.startOnce();
if (steppersOn) {
uint32_t pin_mask_steppers = 0;
//set direction pins
for (int i = 0; i < 4; i++) {
if (curPos[i] != nextPos[i]) {
int8_t sign = -1;
if (nextPos[i] > curPos[i])
sign = 1;
if (sign > 0)
digitalWrite(dir[i], false);
else
digitalWrite(dir[i], true);
curPos[i] = curPos[i] + sign;
pin_mask_steppers = pin_mask_steppers | (1 << step[i]);
}
}
delayMicroseconds(3);
GPIO_REG_WRITE(GPIO_OUT_W1TS_ADDRESS, pin_mask_steppers);
delayMicroseconds(5);
GPIO_REG_WRITE(GPIO_OUT_W1TC_ADDRESS, pin_mask_steppers); // Set pin a and b low
delayMicroseconds(5);
}
}
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 dxl_serial_init(volatile struct dxl_device *device, int index)
{
ASSERT(index >= 1 && index <= 3);
// Initializing device
struct serial *serial = (struct serial*)malloc(sizeof(struct serial));
dxl_device_init(device);
device->data = (void *)serial;
device->tick = dxl_serial_tick;
device->process = process;
serial->index = index;
serial->txComplete = true;
serial->dmaEvent = false;
if (index == 1) {
serial->port = &Serial1;
serial->direction = DIRECTION1;
serial->channel = DMA_CH4;
} else if (index == 2) {
serial->port = &Serial2;
serial->direction = DIRECTION2;
serial->channel = DMA_CH7;
} else {
serial->port = &Serial3;
serial->direction = DIRECTION3;
serial->channel = DMA_CH2;
}
serial->syncReadCurrent = 0xff;
serial->syncReadCount = 0;
initSerial(serial);
if (!serialInitialized) {
serialInitialized = true;
// Initializing DMA
dma_init(DMA1);
usart1_tc_handler = usart1_tc;
usart2_tc_handler = usart2_tc;
usart3_tc_handler = usart3_tc;
// Reset allocation
for (unsigned int k=0; k<sizeof(devicePorts); k++) {
devicePorts[k] = 0;
}
// Initialize sync read timer
syncReadTimer.pause();
syncReadTimer.setPrescaleFactor(CYCLES_PER_MICROSECOND*10);
syncReadTimer.setOverflow(0xffff);
syncReadTimer.refresh();
syncReadTimer.resume();
}
}
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;
}
}
}
}
void baud_timer_restart()
{
#ifdef ___MAPLE
timer4.setCount(0);
timer4.refresh();
#endif
#ifdef ___ARDUINO
TCNT1 = 0;
baudOverflow = false;
#endif
#ifdef ___TEENSY
FTM0_CNT = 0x0;
#endif
}
void OtaUpdate() {
uint8 slot;
rboot_config bootconf;
Serial.println("Updating...");
hardwareTimer.stop();
reportTimer.stop();
sendToClients("Firmware ota update started...");
// need a clean object, otherwise if run before and failed will not run again
if (otaUpdater)
delete otaUpdater;
otaUpdater = new rBootHttpUpdate();
sendToClients("Firmware ota update started...1");
// select rom slot to flash
bootconf = rboot_get_config();
slot = bootconf.current_rom;
if (slot == 0)
slot = 1;
else
slot = 0;
#ifndef RBOOT_TWO_ROMS
// flash rom to position indicated in the rBoot config rom table
otaUpdater->addItem(bootconf.roms[slot], ROM_0_URL);
#else
// flash appropriate rom
if (slot == 0) {
otaUpdater->addItem(bootconf.roms[slot], ROM_0_URL);
} else {
otaUpdater->addItem(bootconf.roms[slot], ROM_1_URL);
}
#endif
#ifndef DISABLE_SPIFFS
// use user supplied values (defaults for 4mb flash in makefile)
if (slot == 0) {
otaUpdater->addItem(RBOOT_SPIFFS_0, SPIFFS_URL);
} else {
otaUpdater->addItem(RBOOT_SPIFFS_1, SPIFFS_URL);
}
#endif
sendToClients("Firmware ota update started...2");
// request switch and reboot on success
otaUpdater->switchToRom(slot);
// and/or set a callback (called on failure or success without switching requested)
otaUpdater->setCallback(OtaUpdate_CallBack);
// start update
sendToClients("Firmware ota update started...3");
otaUpdater->start();
}
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 ppm_decode_go()
{
SerialUSB.print("Starting timer.. rising edge of D");
SerialUSB.print(PPM_PIN);
//start the timer counting.
timer4.resume();
//the timer is now counting up, and any rising edges on D16
//will trigger a DMA request to clone the timercapture to the array
}
void Dynamixel::begin(int baud) {
//TxDString("[DXL]start begin\r\n");
afio_remap(AFIO_REMAP_USART1);//USART1 -> DXL
afio_cfg_debug_ports(AFIO_DEBUG_FULL_SWJ_NO_NJRST);
#ifdef BOARD_CM900 //Engineering version case
gpio_set_mode(PORT_ENABLE_TXD, PIN_ENABLE_TXD, GPIO_OUTPUT_PP);
gpio_set_mode(PORT_ENABLE_RXD, PIN_ENABLE_RXD, GPIO_OUTPUT_PP);
gpio_write_bit(PORT_ENABLE_TXD, PIN_ENABLE_TXD, 0 );// TX Disable
gpio_write_bit(PORT_ENABLE_RXD, PIN_ENABLE_RXD, 1 );// RX Enable
#else
gpio_set_mode(PORT_TXRX_DIRECTION, PIN_TXRX_DIRECTION, GPIO_OUTPUT_PP);
gpio_write_bit(PORT_TXRX_DIRECTION, PIN_TXRX_DIRECTION, 0 );// RX Enable
#endif
/* timer_set_mode(TIMER2, TIMER_CH1, TIMER_OUTPUT_COMPARE);
timer_pause(TIMER2);
uint16 ovf = timer_get_reload(TIMER2);
timer_set_count(TIMER2, min(0, ovf));
timer_set_reload(TIMER2, 10000);//set overflow
ovf = timer_get_reload(TIMER2);
timer_set_compare(TIMER2, TIMER_CH1, min(1000, ovf));
timer_attach_interrupt(TIMER2, TIMER_CH1, TIM2_IRQHandler);
timer_generate_update(TIMER2);
//timer_resume(TIMER2);*/
/*
* Timer Configuation for Dynamixel bus
* 2013-04-03 ROBOTIS Changed it as the below codes
* Dynamixel bus used timer 2(channel 1) to check timeout for receiving data from the bus.
* So, don't use time 2(channel 1) in other parts.
* */
// Pause the timer while we're configuring it
timer.pause();
// Set up period
timer.setPeriod(1); // in microseconds
// Set up an interrupt on channel 1
timer.setMode(TIMER_CH1,TIMER_OUTPUT_COMPARE);
timer.setCompare(TIMER_CH1, 1); // Interrupt 1 count after each update
timer.attachInterrupt(TIMER_CH1,TIM2_IRQHandler);
// Refresh the timer's count, prescale, and overflow
timer.refresh();
// Start the timer counting
//timer.resume();
dxl_initialize(0, baud);
}
/**
* Processing master packets (sending it to the local bus)
*/
static void process(volatile struct dxl_device *self, volatile struct dxl_packet *packet)
{
struct serial *serial = (struct serial*)self->data;
dxl_serial_tick(self);
if (serial->txComplete && !syncReadMode) {
if (packet->instruction == DXL_SYNC_READ && packet->parameter_nb > 2) {
ui8 i;
syncReadMode = true;
syncReadAddr = packet->parameters[0];
syncReadLength = packet->parameters[1];
syncReadDevices = 0;
ui8 total = 0;
for (i=0; (i+2)<packet->parameter_nb; i++) {
ui8 id = packet->parameters[i+2];
if (devicePorts[id]) {
struct serial *port = serials[devicePorts[id]];
if (port->syncReadCount == 0) {
port->syncReadCurrent = 0xff;
syncReadDevices++;
}
port->syncReadIds[port->syncReadCount] = packet->parameters[i+2];
port->syncReadOffsets[port->syncReadCount] = i;
port->syncReadCount++;
total++;
}
}
syncReadTimer.refresh();
syncReadResponse = (struct dxl_packet*)&self->packet;
syncReadResponse->error = 0;
syncReadResponse->parameter_nb = (syncReadLength+1)*total;
syncReadResponse->process = false;
syncReadResponse->id = packet->id;
} else {
// Forwarding the packet to the serial bus
if (packet->id == DXL_BROADCAST || packet->id < 200) {
self->packet.dxl_state = 0;
self->packet.process = false;
sendSerialPacket(serial, packet);
}
}
}
}
//Timer control routines
void baud_timer_init()
{
#ifdef ___MAPLE
timer4.pause(); // Pause the timer while configuring it
timer4.setMode(TIMER_CH1, TIMER_OUTPUT_COMPARE); // Set up interrupt on channel 1
timer4.setCount(0); // Reset count to zero
timer4.setPrescaleFactor(72); // Timer counts at 72MHz/72 = 1MHz 1 count = 1uS
timer4.setOverflow(0xFFFF); // reset occurs at 15.259Hz
timer4.refresh(); // Refresh the timer's count, prescale, and overflow
timer4.resume(); // Start the timer counting
#endif
#ifdef ___ARDUINO
cli(); // stop interrupts during configuration
TCCR1A = 0; // Clear TCCR1A register
TCCR1B = 0; // Clear TCCR1B register
TCNT1 = 0; // Initialize counter value
OCR1A = 0xFFFF; // Set compare match register to maximum value
TCCR1B |= (1 << WGM12); // CTC mode
// We want 1uS ticks, for 16MHz CPU, we use prescaler of 16
// as 1MHz = 1uS period, but Arduino is lame and only has
// 3 bit multiplier, we can have 8 (overflows too quickly)
// or 64, which operates at 1/4 the desired resolution
TCCR1B |= (1 << CS11); // Configure for 8 prescaler
TIMSK1 |= (1 << OCIE1A); // enable compare interrupt
sei(); // re-enable interrupts
#endif
#ifdef ___TEENSY
FTM0_MODE |= FTM_MODE_WPDIS;
FTM0_CNT = 0;
FTM0_CNTIN = 0;
FTM0_SC |= FTM_SC_PS(7);
FTM0_SC |= FTM_SC_CLKS(1);
FTM0_MOD = 0xFFFF;
FTM0_MODE |= FTM_MODE_FTMEN;
/*
PIT_LDVAL1 = 0x500000;
PIT_TCTRL1 = TIE;
PIT_TCTRL1 |= TEN;
PIT_TFLG1 |= 1;
*/
#endif
}
//invoked as configured by the DMA mode flags.
void dma_isr()
{
dma_irq_cause cause = dma_get_irq_cause(DMA1, DMA_CH1);
//using serialusb to print messages here is nice, but
//it takes so long, we may never exit this isr invocation
//before the next one comes in.. (dma is fast.. m'kay)
timer4.setCount(0); // clear counter
if(ppm_timeout) ppm_timeout=0;
switch(cause)
{
case DMA_TRANSFER_COMPLETE:
// Transfer completed
//SerialUSB.println("DMA Complete");
dma_data_captured=1;
break;
case DMA_TRANSFER_HALF_COMPLETE:
// Transfer is half complete
SerialUSB.println("DMA Half Complete");
break;
case DMA_TRANSFER_ERROR:
// An error occurred during transfer
SerialUSB.println("DMA Error");
dma_data_captured=1;
break;
default:
// Something went horribly wrong.
// Should never happen.
SerialUSB.println("DMA WTF");
dma_data_captured=1;
break;
}
}
void FLYMAPLEScheduler::init(void* machtnichts)
{
delay_us(2000000); // Wait for startup so we have time to connect a new USB console
// 1kHz interrupts from systick for normal timers
systick_attach_callback(_timer_procs_timer_event);
// Set up Maple hardware timer for 1khz failsafe timer
// ref: http://leaflabs.com/docs/lang/api/hardwaretimer.html#lang-hardwaretimer
_failsafe_timer.pause();
_failsafe_timer.setPeriod(1000); // 1000us = 1kHz
_failsafe_timer.setChannelMode(TIMER_CH1, TIMER_OUTPUT_COMPARE);// Set up an interrupt on channel 1
_failsafe_timer.setCompare(TIMER_CH1, 1); // Interrupt 1 count after each update
_failsafe_timer.attachInterrupt(TIMER_CH1, _failsafe_timer_event);
_failsafe_timer.refresh();// Refresh the timer's count, prescale, and overflow
_failsafe_timer.resume(); // Start the timer counting
// We run this timer at a higher priority, so that a broken timer handler (ie one that hangs)
// will not prevent the failsafe timer interrupt.
// Caution: the timer number must agree with the HardwareTimer number
nvic_irq_set_priority(NVIC_TIMER2, 0x14);
}
void setup() {
// Set up the LED to blink
pinMode(D13, OUTPUT);
// Pause the timer while we're configuring it
timer.pause();
// Set up period
timer.setPeriod(LED_RATE); // in microseconds
// Set up an interrupt on channel 1
timer.setChannel1Mode(TIMER_OUTPUT_COMPARE);
timer.setCompare(TIMER_CH1, 1); // Interrupt 1 count after each update
timer.attachCompare1Interrupt(handler_led);
// Refresh the timer's count, prescale, and overflow
timer.refresh();
// Start the timer counting
timer.resume();
}
void setup( void )
{
hardwareSetup();
// Setup the LED to steady on
pinMode(BOARD_LED_PIN, OUTPUT);
digitalWrite(BOARD_LED_PIN, HIGH);
// Setup the button as input
pinMode(BOARD_BUTTON_PIN, INPUT);
digitalWrite(BOARD_BUTTON_PIN, HIGH);
// Setup the sensor pin as an analog input
pinMode(sensor_pin,INPUT_ANALOG);
setupKeywords();
registerAction(_DIR_, &DIRaction);
registerAction(_TYP_, &TYPaction);
setupRadioModule();
timer.pause();
// Set up period
timer.setPeriod(1000); // in microseconds
// Set up an interrupt on channel 1
timer.setMode(TIMER_CH1,TIMER_OUTPUT_COMPARE);
timer.setCompare(TIMER_CH1, 1); // Interrupt 1 count after each update
timer.attachInterrupt(TIMER_CH1,ledTask);
// Refresh the timer's count, prescale, and overflow
timer.refresh();
// Start the timer counting
timer.resume();
setupRadioModule();
}
void motor_restart_traj_timer() {
timer3.pause();
timer3.refresh();
timer3.resume();
}
void motor_add_benchmark_time() {
if (timeIndexMotor < TIME_STAMP_SIZE_MOTOR) {
arrayOfTimeStampsMotor[timeIndexMotor] = timer3.getCount();
timeIndexMotor++;
}
}
void motor_init(encoder *pEnc) {
// Ensuring the shut down is active (inversed logic on this one)
digitalWrite(SHUT_DOWN_PIN, LOW);
pinMode(SHUT_DOWN_PIN, OUTPUT);
digitalWrite(SHUT_DOWN_PIN, LOW);
// Preparing the first PWM signal
digitalWrite(PWM_1_PIN, LOW);
pinMode(PWM_1_PIN, PWM);
pwmWrite(PWM_1_PIN, 0x0000);
// Preparing the second PWM signal
digitalWrite(PWM_2_PIN, LOW);
pinMode(PWM_2_PIN, PWM);
pwmWrite(PWM_2_PIN, 0x0000);
if (HAS_CURRENT_SENSING) {
// ADC pin init
pinMode(CURRENT_ADC_PIN, INPUT_ANALOG);
}
// Releasing the shutdown
digitalWrite(SHUT_DOWN_PIN, HIGH);
// Reading the magic offset in the flash
mot.offset = dxl_read_magic_offset();
mot.command = 0;
mot.previousCommand = 0;
mot.predictiveCommand = 0;
mot.predictiveCommandTorque = 0;
mot.angle = pEnc->angle + mot.offset;
mot.previousAngle = pEnc->angle + mot.offset;
mot.angleBuffer =
*buffer_creation(dxl_regs.ram.speedCalculationDelay + 1,
mot.angle); // Works because a tick is 1 ms.
mot.speedBuffer = *buffer_creation(NB_TICKS_BEFORE_UPDATING_ACCELERATION, 0);
mot.targetAngle = pEnc->angle;
mot.speed = 0;
mot.averageSpeed = 0;
mot.previousSpeed = 0;
mot.signOfSpeed = 0;
mot.targetSpeed = 0;
mot.acceleration = 0;
mot.targetAcceleration = 0;
mot.state = COMPLIANT;
mot.current = 0;
mot.averageCurrent = 0;
mot.targetCurrent = 0;
mot.posAngleLimit = 0;
mot.negAngleLimit = 0;
mot.multiTurnOn = false;
mot.multiTurnAngle = mot.angle;
mot.electricalTorque = 0.0;
mot.outputTorque = 0.0;
mot.targetTorque = 0.0;
mot.temperatureIsCritic = false;
// filtre k.p /(1+2*z* p/wn+p2/wn2)
float k = dxl_regs.ram.speedCalculationDelay + 1;
float tau = (dxl_regs.ram.speedCalculationDelay + 1) * 0.5;
float wn = 1 / tau;
float csi = 0.7;
float te = 1;
motor_init_filter_speed(&mot, 0, k, 0, 1, 2 * csi / wn, 1 / (wn * wn), te);
init_filter_state(&mot.filt_speed, mot.angle);
timer3.setPrescaleFactor(
7200); // 1 for current debug, 7200 => 10 tick per ms
timer3.setOverflow(65535);
}
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;
}
}
void connectOk(IPAddress ip, IPAddress mask, IPAddress gateway) {
//void connectOk() {
String ipString = WifiStation.getIP().toString();
Serial.println("I'm CONNECTED to AP_SSID=" + wifi_sid.get(currWifiIndex) + " IP: " + ipString);
//String ipString = ip.toString();
Serial.println("IP: " + ipString);
startWebServer();
if (ipString.equals("192.168.1.115") || ipString.equals("192.168.1.110")) {
// distance sensor
Serial.println("MODE: LEUZE Distance sensor");
udp.listen(udpServerPort);
Serial.begin(57600);
deltat = 100000;
system_uart_swap();
delegateDemoClass.begin();
reportTimer.initializeMs(100, reportAnalogue).start();
} else if (ipString.equals("192.168.1.113") || ipString.equals("192.168.1.112") || ipString.equals("192.168.1.21")
|| ipString.equals("192.168.43.154")) { //
// 4 axis stepper driver
Serial.setCallback(serialCallBack);
Serial.println("MODE: 4 Axis Stepper driver");
Serial.println("Init ended.");
Serial.println("Type 'help' and press enter for instructions.");
Serial.println();
deltat = 2000;
if (ipString.equals("192.168.1.112"))
parseGcode("reassign x=0 y=3 e=1 z=2");
else if (ipString.equals("192.168.1.113"))
parseGcode("reassign x=0 y=1 e=3 z=2");
reportTimer.initializeMs(300, reportStatus).start();
hardwareTimer.initializeUs(deltat, StepperTimerInt);
hardwareTimer.startOnce();
} else if (ipString.equals("192.168.1.116")) {
Serial.println("MODE: Encoder driver");
pinMode(encoder0PinA, INPUT);
digitalWrite(encoder0PinA, HIGH); // turn on pullup resistor
pinMode(encoder0PinB, INPUT);
digitalWrite(encoder0PinB, HIGH); // turn on pullup resistor
attachInterrupt(encoder0PinA, doEncoderA, GPIO_PIN_INTR_ANYEDGE);
attachInterrupt(encoder0PinB, doEncoderB, GPIO_PIN_INTR_ANYEDGE);
reportTimer.initializeMs(100, reportEncoderPosition).start();
} else if (ipString.equals("192.168.1.117")) {
Ltc2400Spi = new SPISoft(PIN_DO, PIN_DI, PIN_CK, PIN_SS);
Ltc2400Spi->begin();
reportTimer.initializeMs(300, readFromLTC2400).startOnce();
}
else
{
Serial.setCallback(serialCallBack);
}
}
void init_ppm_timer()
{
timer4.pause();
timer4.setPrescaleFactor(TIMER_PRESCALE);
timer4.setOverflow(65535);
timer4.setCount(0);
// use channel 2 to detect when we stop receiving
// a ppm signal from the encoder.
timer4.setMode(TIMER_CH2, TIMER_OUTPUT_COMPARE);
timer4.setCompare(TIMER_CH2, 65535);
timer4.attachCompare2Interrupt(ppm_timeout_isr);
timer4.refresh();
//capture compare regs TIMx_CCRx used to hold val after a transition on corresponding ICx
//when cap occurs, flag CCXIF (TIMx_SR register) is set,
//and interrupt, or dma req can be sent if they are enabled.
//if cap occurs while flag is already high CCxOF (overcapture) flag is set..
//CCIX can be cleared by writing 0, or by reading the capped data from TIMx_CCRx
//CCxOF is cleared by writing 0 to it.
//Clear the CC1E bit to disable capture from the counter as we set it up.
//CC1S bits aren't writeable when CC1E is set.
//CC1E is bit 0 of CCER (page 401)
bitClear(r.gen->CCER,0);
//Capture/Compare 1 Selection
// set CC1S bits to 01 in the capture compare mode register 1.
// 01 selects TI1 as the input to use. (page 399 stm32 reference)
// (assuming here that TI1 is D16, according to maple master pin map)
//CC1S bits are bits 1,0
bitClear(r.gen->CCMR1, 1);
bitSet(r.gen->CCMR1, 0);
//Input Capture 1 Filter.
// need to set IC1F bits according to a table saying how long
// we should wait for a signal to be 'stable' to validate a transition
// on the input.
// (page 401 stm32 reference)
//IC1F bits are bits 7,6,5,4
bitClear(r.gen->CCMR1, 7);
bitClear(r.gen->CCMR1, 6);
bitSet(r.gen->CCMR1, 5);
bitSet(r.gen->CCMR1, 4);
//sort out the input capture prescaler IC1PSC..
//00 no prescaler.. capture is done at every edge detected
bitClear(r.gen->CCMR1, 3);
bitClear(r.gen->CCMR1, 2);
//select the edge for the transition on TI1 channel using CC1P in CCER
//CC1P is bit 1 of CCER (page 401)
// 0 = rising (non-inverted. capture is done on a rising edge of IC1)
// 1 = falling (inverted. capture is done on a falling edge of IC1)
bitClear(r.gen->CCER,1);
//set the CC1E bit to enable capture from the counter.
//CC1E is bit 0 of CCER (page 401)
bitSet(r.gen->CCER,0);
//enable dma for this timer..
//sets the Capture/Compare 1 DMA request enable bit on the DMA/interrupt enable register.
//bit 9 is CC1DE as defined on page 393.
bitSet(r.gen->DIER,9);
}
void setup() {
// Setup our pins
pinMode(BOARD_LED_PIN, OUTPUT);
pinMode(VGA_R, OUTPUT);
pinMode(VGA_G, OUTPUT);
pinMode(VGA_B, OUTPUT);
pinMode(VGA_V, OUTPUT);
pinMode(VGA_H, OUTPUT);
digitalWrite(VGA_R, LOW);
digitalWrite(VGA_G, LOW);
digitalWrite(VGA_B, LOW);
digitalWrite(VGA_H, HIGH);
digitalWrite(VGA_V, HIGH);
// Fill the logo array with color patterns corresponding to its
// truth value. Note that we could get more tricky here, since
// there are 3 bits of color.
for (int y = 0; y < y_max; y++) {
for (int x = 0; x < x_max; x++) {
logo[y][x] = logo[y][x] ? ON_COLOR : OFF_COLOR;
}
}
// This gets rid of the majority of the interrupt artifacts;
// there's still a glitch for low values of y, but let's not worry
// about that. (Probably due to the hackish way vsync is done).
SerialUSB.end();
systick_disable();
// Configure
timer.pause(); // while we configure
timer.setPrescaleFactor(1); // Full speed
timer.setMode(TIMER_CH1, TIMER_OUTPUT_COMPARE);
timer.setMode(TIMER_CH2, TIMER_OUTPUT_COMPARE);
timer.setMode(TIMER_CH3, TIMER_OUTPUT_COMPARE);
timer.setMode(TIMER_CH4, TIMER_OUTPUT_COMPARE);
timer.setOverflow(2287); // Total line time
timer.setCompare(TIMER_CH1, 200);
timer.attachInterrupt(TIMER_CH1, isr_porch);
timer.setCompare(TIMER_CH2, 300);
timer.attachInterrupt(TIMER_CH2, isr_start);
timer.setCompare(TIMER_CH3, 2170);
timer.attachInterrupt(TIMER_CH3, isr_stop);
timer.setCompare(TIMER_CH4, 1); // Could be zero, I guess
timer.attachInterrupt(TIMER_CH4, isr_update);
timer.setCount(0); // Ready...
timer.resume(); // Go!
}
/**
* 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;
}
}
}
}
}
