Page Menu
Home
c4science
Search
Configure Global Search
Log In
Files
F73540573
main.c
No One
Temporary
Actions
Download File
Edit File
Delete File
View Transforms
Subscribe
Mute Notifications
Award Token
Subscribers
None
File Metadata
Details
File Info
Storage
Attached
Created
Mon, Jul 22, 12:31
Size
5 KB
Mime Type
text/x-c
Expires
Wed, Jul 24, 12:31 (2 d)
Engine
blob
Format
Raw Data
Handle
19218684
Attached To
R6619 Oncilla Motordriver Firmware
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
Log In to Comment