static void
terminate(int dummy)
{
int i;
dprintf("Resetting DMA...\n");
if (dma_reg && mbox.virt_addr) {
for (i = 0; i < num_channels; i++)
channel_pwm[i] = 0;
update_pwm();
udelay(CYCLE_TIME_US);
dma_reg[DMA_CS] = DMA_RESET;
udelay(10);
}
dprintf("Freeing mbox memory...\n");
if (mbox.virt_addr != NULL) {
unmapmem(mbox.virt_addr, NUM_PAGES * PAGE_SIZE);
if (mbox.handle <= 2) {
/* we need to reopen mbox file */
mbox.handle = mbox_open();
}
mem_unlock(mbox.handle, mbox.mem_ref);
mem_free(mbox.handle, mbox.mem_ref);
mbox_close(mbox.handle);
}
dprintf("Unlink %s...\n", DEVFILE);
unlink(DEVFILE);
dprintf("Unlink %s...\n", DEVFILE_MBOX);
unlink(DEVFILE_MBOX);
printf("pi-blaster stopped.\n");
exit(1);
}
void SysTick_Handler(void) {
tick += 1;
kick_watchdog();
update_pwm();
coilBuzzer();
toggle_dripper();
}
void mosfet_test_update_phase(void)
{
switch(mosfet_test_step){
case 1:
update_pwm(A_B);
mosfet_test_step = 2;
break;
case 2:
update_pwm(OFF,OFF,OFF);
mosfet_test_step = 1;
break;
case 3:
update_pwm(B_C);
mosfet_test_step = 4;
break;
case 4:
update_pwm(OFF,OFF,OFF);
mosfet_test_step = 3;
break;
case 5:
update_pwm(C_A);
mosfet_test_step = 6;
break;
case 6:
update_pwm(OFF,OFF,OFF);
mosfet_test_step = 5;
break;
}
}
int main(int argc, char* argv[]) {
struct servo_shm* val = (struct servo_shm*)parse_attach(argc, argv, 1, sizeof(struct servo_shm));
servo_init(val->channels);
while(1) {
update_pwm();
val->lastUpdate = 0;
msleep(10);
}
return 0;
}
/* Funktionen genomför en PID-reglering */
void pid(void *p)
{
// Initiering av variabler för PID-reglering.
float P = 0.0;
float I = 0.0;
float D = 0.0;
for(;;)
{
// Säkerställer jämn samplingstid.
portTickType xLastWakeTime;
const portTickType xTimeIncrement = SAMPLING_TIME;
/* PID-REGLERING */
// Avstånd läses av och beräknas.
read_sensor(); // Avläser nytt sensorvärde
set_global_d(); // Beräknar nytt avståndsvärde
// Variabler till regleringen beräknas.
eDif = global_e; // Förbereder beräkningen av skillnaden mellan föregående och nuvarande felvärde.
global_e = global_Bv - global_d; // Beräknar felvärdet.
eDif = global_e - eDif; // Beräknar skillnaden mellan föregående och nuvarande felvärde.
// Motverkar skenande I-del.
if(global_u <= 1000.0 && global_u >= 0)
{
eSum = eSum + global_e; // Beräknar summan av samtliga felvärden.
I = (global_dT/global_Ti) * eSum; // Beräknar I-delen.
}
P = global_e; // Beräknar P-delen.
D = (global_Td/global_dT) * eDif; // Beräknar D-delen.
// Styrsignalen beräknas (PID-reglering).
global_u = global_Kp * (P+I+D);
// Styrsignalen skickas som PWM-signal.
update_pwm((uint32_t)global_u);
// Paus som motsvarar samplingstiden.
vTaskDelayUntil(&xLastWakeTime, xTimeIncrement);
}
}
void switch_phase_6_step(void)
{
if(update_rate){
TIM15->ARR = update_rate;
update_rate = 0;
}
TP3_TOGGLE;
// Here to prepare data for next step
if(dir>0) dir = 1;
if(dir<0) dir = -1;
switch(step){
case 1:
SET_PIN(A,15);
update_pwm(A_B);
step += dir;
break;
case 2:
RESET_PIN(A,15);
update_pwm(C_B);
step += dir;
break;
case 3:
update_pwm(C_A);
step += dir;
break;
case 4:
update_pwm(B_A);
step += dir;
break;
case 5:
update_pwm(B_C);
step += dir;
break;
case 6:
update_pwm(A_C);
step += dir;
break;
}
if(step>6){
step = 1;
}else if(step < 1){
step = 6;
}
}
void pwm_force_output(uint8_t a, uint8_t b, uint8_t c)
{
update_pwm(a,b,c);
TIM_GenerateEvent(TIM1, TIM_EventSource_COM);
}
static void
init_pwm(void){
update_pwm();
}
// Set pin2gpio pins, channel width and update the pwm send to pins being used.
static void
set_pwm(int channel, float width)
{
set_pin(channel, width);
update_pwm();
}
// Releases the PWM pin requested (if found and valid) and updates the calls
// update_pwm to apply the changes to the actual hardware pins.
static void
release_pwm(int pin){
release_pin(pin);
update_pwm();
}
int main()
{
unsigned char cmd_buf[8];
int cmd_len;
int cmd_len_expected;
int cmd_state;
int status;
clock_init();
AD1PCFGL = 0xFFFF; // All pins as digital.
// UART pins/mapping
TRISBbits.TRISB3 = 1;
RPOR1bits.RP2R = 3; // Assign UART1 TX to RP2 (pin 6).
RPINR18bits.U1RXR = 3; // Assign UART1 RX to RP3 (pin 7).
serial_init();
// PWM pins/mapping
TRISBbits.TRISB15 = 0;
TRISBbits.TRISB14 = 0;
TRISBbits.TRISB13 = 0;
TRISBbits.TRISB12 = 0;
RPOR7 = 0b0001001000010011; // RP15 - OC1, RP14 - OC2
RPOR6 = 0b0001010000010101; // RP13 - OC3, RP12 - OC4
_pwmChannel1 = 0;
_pwmChannel2 = 0;
_pwmChannel3 = 0;
_pwmChannel4 = 0;
_pwmFailsafeEnabled = 0;
pwm_init();
update_pwm();
// Set up timer 3 failsafe to determine when commands are no longer
// being sent. When triggered, gradually reduce PWM values until
// the next command is sent or the PWM value reaches minimum.
T3CON = 0;
T3CONbits.TCKPS = 0b11; // 1:256 prescale
IFS0bits.T3IF = 0; // Clear interrupt flag
IEC0bits.T3IE = 1; // Enable timer interrupt
TMR3 = 0;
PR3 = 0xFFFF;
_timer3high = 0; // High word to create 32-bit timer from 16-bit timer
T3CONbits.TON = 0b1; // Enable timer 3
cmd_len = 0;
cmd_len_expected = 0;
cmd_state = CMD_STATE_NONE;
while (1)
{
update_pwm();
status = serial_readchar(&cmd_buf[cmd_len]);
if (status == SERIAL_STATUS_NODATA)
{
continue;
}
else if (status == SERIAL_STATUS_ERROR)
{
cmd_state = CMD_STATE_NONE;
cmd_len = 0;
cmd_len_expected = 0;
}
else if (status == SERIAL_STATUS_DATA)
{
cmd_len++;
if (cmd_len >= 8)
{
// Command data overflow.
cmd_state = CMD_STATE_NONE;
cmd_len = 0;
cmd_len_expected = 0;
continue;
}
if (cmd_state == CMD_STATE_NONE)
{
// New command - check start byte.
if (cmd_buf[0] == CMD_SETDUTYCYCLES)
{
cmd_state = CMD_STATE_READING;
cmd_len_expected = CMD_SETDUTYCYCLES_LEN;
}
else
{
// Invalid command.
cmd_state = CMD_STATE_NONE;
cmd_len = 0;
cmd_len_expected = 0;
}
}
// Check for data portion of command (if any).
if (cmd_state == CMD_STATE_READING)
{
if (cmd_len - 1 == cmd_len_expected)
{
process_command(cmd_buf);
cmd_state = CMD_STATE_NONE;
cmd_len = 0;
cmd_len_expected = 0;
}
}
}
}
return 0;
}
