Page MenuHomec4science
Files F70984776

main.c
No OneTemporary
Actions

File Metadata

Created
Mon, Jul 8, 15:15

main.c
View Options

#include <p33FJ128MC804.h>
#include "config.h"
#include <libpic30.h>
#include <dsp.h>
#include "pindefs.h"
#include "misc.h"
#include "motorcontrol.h"
#include "timer.h"
#include "uart.h"
#include "dma_sbcp.h"
#include "dma_spi.h"
#include "sbcp.h"
//#include "cpg.h"
//#include "sine.h"
#include "control_table.h"
#include "command.h"
#include "sbcp_ll.h"
/** C O N F I G ****************************************************/
// See "p33fj128mc804.h" for all config settings.
_FGS(GSS_OFF & GWRP_OFF); // Code Protect off, Write protect disabled
_FOSCSEL(FNOSC_PRIPLL); // Prim. Osc (XT, HS, EC) w/ PLL, Two speed osc off, Temp protect off
_FOSC(FCKSM_CSDCMD & OSCIOFNC_ON & POSCMD_EC); // Clock Switch & Clock Monitor off, OSC2 is IO, external clock generation
_FWDT(FWDTEN_OFF); // Watchdog Timer Disabled
_FPOR(FPWRT_PWR1); // No PWM off delay, PWM controlled by port configuration at device reset
_FICD(JTAGEN_OFF & ICS_PGD1); // No JTAG, debugging on PGD1 pins
// create 2 motor structures
extern motor M1, M2;
extern unsigned int volatile systime;
unsigned int prev_systime;
extern table_field control_table[A_MDRV_TABLE_SIZE];
void clock_init() {
// Configure PLL prescaler, PLL postscaler and PLL divisor
// 8MHz oscillator
// frequency = Fin * M / (N1 * N2)
// Maximum frequency is 80MHz = 40MIPS
// => 8*40/(2*2)
PLLFBD = 38; // M = 40
CLKDIVbits.PLLPOST=0; // N2 = 2
CLKDIVbits.PLLPRE=0; // N1 = 2
}
void pwm_init() {
// initialize PWM channels
// ***************** PWM section *****************
// P1TCON, P2TCON
// -> pwm enabled, pwm stops in idle mode, postscale 1:1, prescale 1:1, free run mode
P1TCON = 0b1000000000000000; // PTEN (1), DC (1), PTSIDL (1), DC (5), PTOPS (4), PTCKPS (2), PTMOD (2)
P2TCON = 0b1000000000000000; // PTEN (1), DC (1), PTSIDL (1), DC (5), PTOPS (4), PTCKPS (2), PTMOD (2)
// PWM1CON1, PWM2CON1
// -> only high side pwm channel is used
PWM1CON1 = 0b0000011101110000; // DC (5), independent mode (3), DC (1), enable PWM1Hx (3), DC (1), PWM1Lx is IO-pin (3)
PWM2CON1 = 0b0000000100010000; // DC (7), independent mode (1), DC (3), enable PWM2Hx (1), DC (3), PWM2Lx is IO-pin (1)
// P1TPER, P2TPER
// see p.14-25 ref manual motorcontrol dspic33f
P1TPER = MAX_PWM;
P2TPER = MAX_PWM;
// initialize as 0 voltage
PWM_TRQ1 = 0;
M1_SET_PWM(0);
PWM_TRQ2 = 0;
M2_SET_PWM(0);
}
void qei_init() {
// Enable pullup on B8-B11 (encoder inputs)
// __builtin_write_OSCCONL(0b10000000);//unlock peripheral pinsettings (p.168 manual)
_QEA1R = 20;
_QEB1R = 21;
_QEA2R = 6;
_QEB2R = 7;
// __builtin_write_OSCCONL(0b11000000);//lock pereriphal pinsettings
// ***************** QEI section *****************
QEI1CON = 0b0000011100000000; // CNTERR(1), unused(1), QEISIDL(1), INDEX(1), UPDN(1), QEIM(3), SWPAB(1), PCDOUT(1), TQGATE(1), POSRES(2) (prescale), POSRES(1), TQCS(1), UPDN_SRC(1)
DFLT1CON = 0b0000000110100000; // unused(5), IMV(2), CEID(1), QEOUT(1), QECK(3), unused(4) -> digital filter 1:4
MAX1CNT = 0xffff;//maxcount for qei position.
QEI2CON = 0b0000011100000000;
DFLT2CON = 0b0000000110100000;
MAX2CNT = 0xffff;//maxcount for qei position.
M1.previous_cnt = 0;
M2.previous_cnt = 0;
}
void delay_ms(unsigned int t) {
uint16 i;
for (i=0; i<t; i++) {
__delay_ms(1);
}
}
///////////////////////////////////////////////////////////////////////////////
int main (void) {
int i;
// enable motordrivers reset
MRST_ENABLE;
// wait a little bit
delay_ms(100);
clock_init();
pin_init();
pwm_init();
qei_init();
init_control_table();
init_motor(&M1);
init_motor(&M2);
// RS485 communication init
timer_init();
uart_init();
dma_sbcp_init();
sbcp_init();
spi_init();
// init_message_frame();
prev_systime = 0;
// disable motordrivers reset
MRST_DISABLE;
// enable motor 1
BRAKE1 = 1; // active low
COAST1 = 1; // active low
// enable motor 2
BRAKE2 = 1; // active low
COAST2 = 1; // active low
// load motor default control parameters
M1_load_control_table_settings();
M2_load_control_table_settings();
//create the cpg context
// struct cpg_globals cpg;
//init the context
// cpg_init(&cpg);
//create the sine wave context
//struct sine_globals sine;
//init the context
//sine_init(&sine);
// initialize the magnetic encoders and set the limit values
//unsigned int buffer_tmp[200];
//command_calibrate(buffer_tmp);
// MRST_ENABLE;
/*
sine_move_to_initial_position(&sine);
// wait till motors are not moving anymore
while (M1.moving_en || M2.moving_en) {
if (systime != prev_systime) {
prev_systime = systime;
motorcontrol_main();
}
}
// hardware hack, since the sine_main was not running until now
sine.next_send_time = systime + sine.period;
*/
while (1) {
// run indefinetly
// check if system time has changed
if (systime != prev_systime) {
prev_systime = systime;
// order of processes determines priority
// motor control process is most sensitive to timing
motorcontrol_main();
//sensor reading process, just meke one reading each 1ms
spi_start_reading();
// cpg_main(&cpg);
// sine_main(&sine);
}
// put here processes which do not depend on the system timer
if (systime == prev_systime) {
spi_main(); //will process sensor data if needed
sbcp_low_latency_packet_main();
sbcp_main();
}
}
}

Event Timeline

c4science · Help