int16_t main(void) {
//initialize all system clocks
init_clock();
//initialize serial communications
init_uart();
//initialize pin driving library (to be able to use the &D[x] defs)
init_pin();
//initialize the UI library
init_ui();
//initialize the timer module
init_timer();
//initialize the OC module (used by the servo driving code)
init_oc();
imu_init()
//Set servo control pins as output
pin_digitalOut(PAN_PIN);
pin_digitalOut(TILT_PIN);
pin_digitalOut(SONIC_OUT_PIN);
pin_digitalIn(SONIC_IN_PIN);
//Set LED off
led_off(LED);
//Configure blinking rate for LED when connected
timer_setPeriod(LED_TIM, 0.2);
timer_start(LED_TIM);
//Configure timer for reciever timeout
timer_setPeriod(DIST_TIM, 0.05);
//configure PWM on sonic output pin
oc_pwm(PWM_OC, SONIC_OUT_PIN, NULL, SONIC_FREQ, 0x0000);
//According to HobbyKing documentation: range .8 through 2.2 msec
//Set servo control pins as OC outputs on their respective timers
oc_servo(SERVO1_OC, PAN_PIN, SERVO1_TIM, SERVO_PERIOD, SERVO_MIN, SERVO_MAX, pan_set_val);
oc_servo(SERVO2_OC, TILT_PIN, SERVO2_TIM, SERVO_PERIOD, SERVO_MIN, SERVO_MAX, tilt_set_val);
InitUSB(); // initialize the USB registers and serial interface engine
while (USB_USWSTAT!=CONFIG_STATE) { // while the peripheral is not configured...
ServiceUSB(); // ...service USB requests
led_on(LED);
//There's no point in driving the servos when there's no one connected yet.
}
while (1) {
ServiceUSB(); // service any pending USB requests
//blink the LED
if (timer_flag(LED_TIM)) {
timer_lower(LED_TIM);
led_toggle(LED);
}
//Update the servo control values.
x_gout = gyro_read(OUT_X_L);
}
}
void spi_close(_SPI *self) {
*(self->SPIxSTAT) = 0;
*(self->SPIxCON1) = 0;
*(self->SPIxCON2) = 0;
if (self->MISO) {
__builtin_write_OSCCONL(OSCCON&0xBF);
*(self->MISOrpinr) |= 0x3F<<(self->MISOrpshift);
__builtin_write_OSCCONL(OSCCON|0x40);
self->MISO->owner = NULL;
pin_digitalIn(self->MISO);
self->MISO = NULL;
}
if (self->MOSI) {
__builtin_write_OSCCONL(OSCCON&0xBF);
*(self->MOSI->rpor) &= ~(0x3F<<(self->MOSI->rpshift));
__builtin_write_OSCCONL(OSCCON|0x40);
self->MOSI->owner = NULL;
pin_digitalOut(self->MOSI);
pin_set(self->MOSI);
self->MOSI = NULL;
}
if (self->SCK) {
__builtin_write_OSCCONL(OSCCON&0xBF);
*(self->SCK->rpor) &= ~(0x3F<<(self->SCK->rpshift));
__builtin_write_OSCCONL(OSCCON|0x40);
self->SCK->owner = NULL;
pin_digitalOut(self->SCK);
pin_clear(self->SCK);
self->SCK = NULL;
}
}
void init_segment(void){
segmentClock = &D[0];
segmentLatch = &D[1];
segmentData = &D[2];
pin_digitalOut(segmentClock);
pin_digitalOut(segmentLatch);
pin_digitalOut(segmentData);
pin_clear(segmentClock);
pin_clear(segmentLatch);
pin_clear(segmentData);
}
void init_audio(_PIN *request_pin) {
audio.play = START;
pin_digitalOut(request_pin);
audio.request_pin = request_pin;
pin_clear(audio.request_pin);
}
void setup(void) {
// Initialize PIC24 modules.
init_clock();
init_ui();
init_timer();
init_pin();
init_oc();
init_spi();
init_uart();
// Configure single SPI comms. system
pin_digitalOut(SPI_CS);
pin_set(SPI_CS);
spi_open(spi_inst, SPI_MISO, SPI_MOSI, SPI_SCK, spi_freq, spi_mode);
// Configure & start timers used.
timer_setPeriod(&timer1, 1);
timer_setPeriod(&timer2, 1); // Timer for LED operation/status blink
timer_setPeriod(&timer3, LOOP_TIME); // Timer for main control loop
timer_start(&timer1);
timer_start(&timer2);
timer_start(&timer3);
// Configure dual PWM signals for bidirectional motor control
oc_pwm(&oc1, PWM_I1, NULL, pwm_freq, pwm_duty);
oc_pwm(&oc2, PWM_I2, NULL, pwm_freq, pwm_duty);
InitUSB(); // initialize the USB registers and
// serial interface engine
while (USB_USWSTAT != CONFIG_STATE) { // while periph. is not configured,
ServiceUSB(); // service USB requests
}
}
int16_t main(void) {
init_clock();
init_ui();
init_pin();
init_spi();
ENC_MISO = &D[1];
ENC_MOSI = &D[0];
ENC_SCK = &D[2];
ENC_NCS = &D[3];
pin_digitalOut(ENC_NCS);
pin_set(ENC_NCS);
spi_open(&spi1, ENC_MISO, ENC_MOSI, ENC_SCK, 2e8);
InitUSB(); // initialize the USB registers and serial interface engine
while (USB_USWSTAT!=CONFIG_STATE) { // while the peripheral is not configured...
ServiceUSB(); // ...service USB requests
}
while (1) {
ServiceUSB(); // service any pending USB requests
}
}
void setup(void) {
// Initialize PIC24 modules.
init_clock();
init_ui();
init_timer();
init_pin();
init_oc();
init_spi();
init_uart();
// Configure single SPI comms. system
pin_digitalOut(SPI_CS);
pin_set(SPI_CS);
spi_open(spi_inst, SPI_MISO, SPI_MOSI, SPI_SCK, spi_freq, spi_mode);
// Configure & start timers used.
timer_setPeriod(&timer1, 1);
timer_setPeriod(&timer2, 1); // Timer for LED operation/status blink
timer_setPeriod(&timer3, LOOP_TIME); // Timer for main control loop
timer_start(&timer1);
timer_start(&timer2);
timer_start(&timer3);
// Configure motor current conversion coefficient
CURRENT_CONV_COEF = MAX_ADC_OUTPUT * MOTOR_VOLTAGE_RESISTOR;
cur_control.Kp = KP;
cur_control.Kd = KD;
cur_control.Ki = KI;
cur_control.dt = LOOP_TIME;
cur_control.integ_min = -100;
cur_control.integ_max = 100;
cur_control.integ_state = 0;
read_motor_current(&motor);
cur_control.prev_position = convert_motor_torque(motor.current);
// Configure dual PWM signals for bidirectional motor control
oc_pwm(&oc1, PWM_I1, NULL, pwm_freq, pwm_duty);
oc_pwm(&oc2, PWM_I2, NULL, pwm_freq, pwm_duty);
pin_analogIn(MOTOR_VOLTAGE);
pin_digitalOut(DEBUGD0);
pin_digitalOut(DEBUGD1);
InitUSB(); // initialize the USB registers and
// serial interface engine
while (USB_USWSTAT != CONFIG_STATE) { // while periph. is not configured,
ServiceUSB(); // service USB requests
}
}
int16_t main(void) {
init_clock();
init_timer();
init_ui();
init_pin();
init_spi();
init_oc();
init_md();
// Current measurement pin
pin_analogIn(&A[0]);
// SPI pin setup
ENC_MISO = &D[1];
ENC_MOSI = &D[0];
ENC_SCK = &D[2];
ENC_NCS = &D[3];
pin_digitalOut(ENC_NCS);
pin_set(ENC_NCS);
// Open SPI in mode 1
spi_open(&spi1, ENC_MISO, ENC_MOSI, ENC_SCK, 2e6, 1);
// Motor setup
md_velocity(&md1, 0, 0);
// Get initial angle offset
uint8_t unset = 1;
while (unset) {
ANG_OFFSET = enc_readReg((WORD) REG_ANG_ADDR);
unset = parity(ANG_OFFSET.w);
}
ANG_OFFSET.w &= ENC_MASK;
// USB setup
InitUSB();
while (USB_USWSTAT!=CONFIG_STATE) {
ServiceUSB();
}
// Timers
timer_setFreq(&timer2, READ_FREQ);
timer_setFreq(&timer3, CTRL_FREQ);
timer_start(&timer2);
timer_start(&timer3);
// Main loop
while (1) {
ServiceUSB();
if (timer_flag(&timer2)) {
timer_lower(&timer2);
get_readings();
}
if (timer_flag(&timer3)) {
timer_lower(&timer3);
set_velocity();
}
}
}
void init_motor_swing(void){
dirpin = &D[8]; //set direction control pin as pin 8
pwmpin = &D[9]; //set PWM pin as pin 9
potentiometer = &A[5]; //read potentiometer pin as pin 5
//initialize the two pins as digital outputs and then clear them
pin_digitalOut(dirpin);
pin_digitalOut(pwmpin);
//initialize analog input for potentiometer
pin_analogIn(potentiometer);
pin_clear(dirpin);
pin_clear(pwmpin);
oc_pwm(&oc1, pwmpin, &timer5, 1e3, 0);
}
void init_launcher(_PIN *load_sensor, _PIN *launch_sensor, _PIN *launch_motor, _PIN *elevator_motor) {
pin_digitalIn(load_sensor);
pin_digitalIn(launch_sensor);
pin_digitalOut(launch_motor);
pin_digitalOut(elevator_motor);
launcher.load_sensor = load_sensor;
launcher.launch_sensor = launch_sensor;
launcher.launch_motor = launch_motor;
launcher.elevator_motor = elevator_motor;
launcher.over = 1;
launcher.launch = 0;
launcher.loaded = 0;
launcher.state = over_launcher;
launcher.last_state = (STATE_HANDLER_T)NULL;
}
//OTHER INITIAL CONDITIONS
void initChip(){
init_clock();
init_uart();
init_timer();
init_ui();
init_pin();
init_oc();
pin_analogIn(CUR);
pin_analogIn(EMF);
pin_analogIn(FB);
pin_digitalIn(SF);
pin_digitalIn(ENC);
pin_digitalOut(D1);
pin_digitalOut(D2);
pin_digitalOut(IN1);
pin_digitalOut(IN2);
pin_digitalOut(ENA);
pin_digitalOut(SLEW);
pin_digitalOut(INV);
oc_pwm(&oc1, D2, PWM_TIMER, 250, 0);
}
int16_t main(void) {
init_clock();
init_ui();
init_pin();
init_spi();
init_timer();
init_oc();
init_md();
led_off(&led2);
led_off(&led3);
ENC_MISO = &D[1];
ENC_MOSI = &D[0];
ENC_SCK = &D[2];
ENC_NCS = &D[3];
read_angle.w=0x3FFF;
Pscale.w=1;
Iscale.w=0;
direction.w=1;
speed.w=0;
angle_now.i=180;
angle_prev.i=180;
angle.w=10;
uint8_t loop = 0;
pin_digitalOut(ENC_NCS);
pin_set(ENC_NCS);
spi_open(&spi1, ENC_MISO, ENC_MOSI, ENC_SCK, 2e8,1);
timer_setPeriod(&timer1, 0.05);
timer_start(&timer1);
InitUSB();
// initialize the USB registers and serial interface engine
while (USB_USWSTAT!=CONFIG_STATE) { // while the peripheral is not configured...
ServiceUSB(); // ...service USB requests
}
while(1){
ServiceUSB();
if (timer_flag(&timer1)) {
timer_lower(&timer1);
angle_prev=angle_now;
angle_prev_con=angle; // service any pending USB requests
angle_now = enc_readReg(read_angle);
angle_now = mask_angle(angle_now);
angle=convert_Angle(angle_prev,angle_now,&loop);
spring_simple(angle);
}
}
}
void init_as5048(_AS5048 *self, _Pin *CS)
{
self->ErrorFlag =0;
self->RawAngle=0;
self->CS=CS_pin;
pin_digitalOut(self->CS);
pin_clear(self->CS);
}
int16_t main(void) {
init_clock();
init_uart();
init_pin();
pin_digitalOut(&D[0]);
while (1) {
pin_toggle(&D[0]);
}
}
void init_motor(void){
//outputs
pin_digitalOut(IN1); //D2-bar
pin_write(IN1,1);
pin_digitalOut(IN2); //D2-bar
pin_write(IN2,0);
pin_digitalOut(D1); //D1
pin_write(D1,0); //no tri-stating!
pin_digitalOut(ENA); //ENA
pin_write(ENA,1); //Enable the system
pin_digitalOut(&D[7]); //SLEW
pin_write(&D[7],0); //low slew rate
pin_digitalOut(INV); //INV
pin_write(INV,0); //don't invert the inputs!
//inputs
pin_analogIn(CURRENT_PIN); //direction sensor
pin_analogIn(VEMF_PIN); //Vemf sensor
pin_analogIn(FB_PIN); //0.24% of active high side current
pin_digitalIn(REV_PIN); //tach input
}
void imu_init(void) {
pin_init(&IMU_MOSI, (unsigned int *)&PORTB, (unsigned int *)&TRISB,
(unsigned int *)NULL, 8, -1, 0, 8, (unsigned int *)&RPOR4);
pin_init(&IMU_SCK, (unsigned int *)&PORTB, (unsigned int*)&TRISB,
(unsigned int *)NULL, 9, -1, 8, 9, (unsigned int*)&RPOR4);
pin_init(&IMU_MISO, (unsigned int *)&PORTB, (unsigned int *)&TRISB,
(unsigned int *)NULL, 14, -1, 0, 14, (unsigned int *)&RPOR7);
pin_init(&ACCEL_CS, (unsigned int*)&PORTB, (unsigned int *)&TRISB,
(unsigned int *)NULL, 13, -1, 0, -1, (unsigned int *)NULL);
pin_init(&GYRO_CS, (unsigned int *)&PORTB, (unsigned int *)&TRISB,
(unsigned int *)NULL, 11, -1, 0, -1, (unsigned int *)NULL);
pin_digitalOut(&ACCEL_CS);
pin_digitalOut(&GYRO_CS);
pin_set(&GYRO_CS);
pin_set(&ACCEL_CS);
spi_open(&spi1, &IMU_MISO, &IMU_MOSI, &IMU_SCK, 2e6);
accel_write(I2CADD, 0x80); //Disable I2C
}
int16_t main(void) {
init_clock();
init_timer();
init_pin();
init_oc();
init_ui();
InitUSB(); // initialize the USB registers and serial interface engine
while (USB_USWSTAT!=CONFIG_STATE) { // while the peripheral is not configured...
ServiceUSB(); // ...service USB requests
}
// Configure Interrupts on the pic
IEC1bits.CNIE = 1;
CNEN1bits.CN2IE = 1;
IFS1bits.CNIF = 0;
IEC0bits.OC1IE = 1;
IFS0bits.OC1IF = 0;
timer_enableInterrupt(&timer1);
timer_lower(&timer1);
timer_enableInterrupt(&timer2);
timer_lower(&timer2);
timer_enableInterrupt(&timer4);
timer_lower(&timer4);
timer_enableInterrupt(&timer5);
timer_lower(&timer5);
// Configure Pins
inPin0 = &A[0];
pin_analogIn(inPin0);
inPin1 = &A[1];
pin_analogIn(inPin1);
inPin2 = &A[2];
pin_analogIn(inPin2);
inPin3 = &A[3];
pin_analogIn(inPin3);
inPin4 = &A[4];
pin_analogIn(inPin4);
irPin = &A[5];
pin_analogIn(irPin);
outPin = &D[6];
pin_digitalOut(outPin);
oc_pwm(&oc1, outPin, NULL, 10, (uint16_t)(0)); // write to D2 with a 10Hz PWM signal
pin_write(outPin, 10000); //duty doesn't matter, really.
redPin = &D[7];
pin_digitalOut(redPin);
oc_pwm(&oc2, redPin, NULL, 100, (uint16_t)(0));
greenPin = &D[10];
pin_digitalOut(greenPin);
oc_pwm(&oc3, greenPin, NULL, 100, (uint16_t)(0));
bluePin = &D[8];
pin_digitalOut(bluePin);
oc_pwm(&oc4, bluePin, NULL, 100, (uint16_t)(0));
pingPin = &D[4];
pin_digitalOut(pingPin);
oc_pwm(&oc5, pingPin, &timer3, 40000, 0);
receivePin = &D[12];
pin_digitalIn(receivePin);
// Motor controller pins
dirPin = &D[0];
pin_digitalOut(dirPin);
nSleepPin = &D[3];
pin_digitalOut(nSleepPin);
pin_write(nSleepPin, 1);
stepPin = &D[2];
pin_digitalOut(stepPin);
testPin = &D[13];
pin_digitalOut(testPin);
timer_setFreq(&timer1, 100);
while (1) {
ServiceUSB(); // service any pending USB requests
irVoltage = pin_read(irPin);
if (irVoltage < 40000){
dist = 32768;
}
if (irVoltage >= 40000){
dist = 32900;
}
if (dist != stepCount) {
changeFlag += 1;
} else {
changeFlag = 0;
}
if (changeFlag >= 3){
changeFlag = 0;
motorControl(dist);
}
if (touching0 == 10){
greenTarget = 40000;
redTarget = 60000;
blueTarget = 0;
if (currentPetal == 0){
greenTarget = 0;
redTarget = 0;
blueTarget = 0;
}
currentPetal == 0;
}
if (touching1 == 11){
greenTarget = 20000;
redTarget = 20000;
blueTarget = 20000;
if (currentPetal == 1){
greenTarget = 0;
redTarget = 0;
blueTarget = 0;
}
currentPetal == 1;
}
if (touching2 == 12){
greenTarget = 0;
redTarget = 60000;
blueTarget = 40000;
if (currentPetal == 2){
greenTarget = 0;
redTarget = 0;
blueTarget = 0;
}
currentPetal == 2;
}
if (touching3 == 13){
greenTarget = 0;
redTarget = 60000;
blueTarget = 0;
if (currentPetal == 3){
greenTarget = 0;
redTarget = 0;
blueTarget = 0;
}
currentPetal == 3;
}
if (touching4 == 14){
greenTarget = 60000;
redTarget = 0;
blueTarget = 0;
if (currentPetal == 4){
greenTarget = 0;
redTarget = 0;
blueTarget = 0;
}
currentPetal == 4;
}
if (greenDuty < greenTarget) {
greenChange = 1;
} else if (greenDuty > greenTarget) {
greenChange = -1;
} else {
greenChange = 0;
onTarget += 1;
}
if (redDuty < redTarget) {
redChange = 1;
} else if (redDuty > redTarget) {
redChange = -1;
} else {
redChange = 0;
onTarget += 1;
}
if (blueDuty < blueTarget) {
blueChange = 1;
} else if (blueDuty > blueTarget) {
blueChange = -1;
} else {
blueChange = 0;
onTarget += 1;
}
greenDuty += greenChange;
redDuty += redChange;
blueDuty += blueChange;
pin_write(greenPin, greenDuty);
pin_write(redPin, redDuty);
pin_write(bluePin, blueDuty);
/*
// fade on when touched
if (touching0 == 10){
if (greenOn == 0){
greenChange = 1;
}
if (greenOn == 1){
greenChange = -1;
}
redChange = -1;
blueChange = -1;
}
if (touching1 == 11){
if (redOn == 1){
redChange = -1;
}
if (redOn == 0){
redChange = 1;
}
blueChange = -1;
greenChange = -1;
}
if (touching2 == 12){
if (blueOn == 1){
blueChange = -1;
}
if (blueOn == 0){
blueChange = 1;
}
greenChange = -1;
redChange = -1;
}
greenDuty = greenDuty + greenChange;
if (greenDuty == MAX_INT){
greenDuty = MAX_INT -1;
greenOn = 1;
greenChange = 0;
}
if (greenDuty == 0){
greenDuty = 1;
greenOn = 0;
greenChange = 0;
}
redDuty = redDuty + redChange;
if (redDuty == MAX_INT){
redDuty = MAX_INT - 1;
redOn = 1;
redChange = 0;
}
if (redDuty == 0){
redDuty = 1;
redOn = 0;
redChange = 0;
}
blueDuty = blueDuty + blueChange;
if (blueDuty == MAX_INT){
blueDuty = MAX_INT - 1;
blueOn = 1;
blueChange = 0;
}
if (blueDuty == 0){
blueDuty = 1;
blueOn = 0;
blueChange = 0;
}
pin_write(greenPin, greenDuty);
pin_write(redPin, redDuty);
pin_write(bluePin, blueDuty);
*/
/*
if (iteration > 10000) {
ping();
iteration = 0;
}
iteration += 1;
*/
}
}
void spi_open(_SPI *self, _PIN *MISO, _PIN *MOSI, _PIN *SCK, float freq) {
uint16_t primary, secondary;
if ((MISO->rpnum==-1) || (MOSI->rpnum==-1) || (SCK->rpnum==-1))
return; // At least one of the specified pins is not an RP pin
if ((MISO->owner==NULL) && (MOSI->owner==NULL) && (SCK->owner==NULL)) {
// All of the specified pins are available and RP pins, so configure
// as specified
pin_digitalIn(MISO);
pin_digitalOut(MOSI);
pin_set(MOSI);
pin_digitalOut(SCK);
pin_clear(SCK);
self->MISO = MISO;
MISO->owner = (void *)self;
MISO->write = NULL;
MISO->read = NULL;
self->MOSI = MOSI;
MOSI->owner = (void *)self;
MOSI->write = NULL;
MOSI->read = NULL;
self->SCK = SCK;
SCK->owner = (void *)self;
SCK->write = NULL;
SCK->read = NULL;
__builtin_write_OSCCONL(OSCCON&0xBF);
*(self->MISOrpinr) &= ~(0x3F<<(self->MISOrpshift));
*(self->MISOrpinr) |= (MISO->rpnum)<<(self->MISOrpshift);
*(MOSI->rpor) &= ~(0x3F<<(MOSI->rpshift));
*(MOSI->rpor) |= (self->MOSIrpnum)<<(MOSI->rpshift);
*(SCK->rpor) &= ~(0x3F<<(SCK->rpshift));
*(SCK->rpor) |= (self->SCKrpnum)<<(SCK->rpshift);
__builtin_write_OSCCONL(OSCCON|0x40);
} else if ((self->MISO!=MISO) || (self->MOSI!=MOSI) || (self->SCK!=SCK)) {
return; // At least one of the specified pins does not match the
// previous assignment
}
// Clip freq to be in allowable range of values
if (freq>(FCY/2.))
freq = FCY/2.;
if (freq<(FCY/(64.*8.)))
freq = FCY/(64.*8.);
// Select primary prescale bits
if (freq<=(FCY/(2.*64.))) {
freq *= 64.;
primary = 0; // Set primary prescale bits for 64:1
} else if (freq<=(FCY/(2.*16.))) {
freq *= 16.;
primary = 1; // Set primary prescale bits for 16:1
} else if (freq<=(FCY/(2.*4.))) {
freq *= 4.;
primary = 2; // Set primary prescale bits for 4:1
} else {
primary = 3; // Set primary prescale bits for 1:1
}
// Compute secondary prescale value to get closest SPI clock freq to that
// specified
secondary = (uint16_t)(0.5+FCY/freq);
secondary = (8-secondary)<<2; // Map secondary prescale bits for SPIxCON1
// Configure the SPI module
// set SPI module to 8-bit master mode
// set SMP = 0, CKE = 1, and CKP = 0
// set SPRE and PPRE bits to get the closest SPI clock freq to that
// specified
*(self->SPIxCON1) = 0x0120|primary|secondary;
*(self->SPIxCON2) = 0;
// Enable the SPI module and clear status flags
*(self->SPIxSTAT) = 0x8000;
}
