void get_readings() {
/*
Get readings for current, raw angle, wraps, unwrapped angle, and velocity
*/
// Read current pin and zero-center
CURRENT.w = pin_read(&A[0]) - CUR_OFFSET;
// Read the encoder, check parity, and subtract initial offset
LAST_ANGLE = ANGLE;
WORD result = enc_readReg((WORD) REG_ANG_ADDR);
if (!parity(result.w)) {
ANGLE.w = ((result.w & ENC_MASK) - ANG_OFFSET.w) & ENC_MASK;
}
// Check for wrapping; if the angle jumps from <~20 degrees
// to >~340 degrees, or vice-versa, we wrapped
if ((ANGLE.w < 0x038E) && (LAST_ANGLE.w > 0x3C71)) {
WRAPS += 1;
} else if ((ANGLE.w > 0x3C71) && (LAST_ANGLE.w < 0x038E)) {
WRAPS -= 1;
}
// Apply wrapping to the raw angle
uint16_t last = UNWRAPPED_ANGLE.w;
UNWRAPPED_ANGLE.w = ANGLE.w + ENC_MASK * WRAPS;
// Calculate velocity as (change in angle) / (time between readings)
// Divide by 16 to avoid overflow
VELOCITY.w = (UNWRAPPED_ANGLE.w - last) * (READ_FREQ / 16);
}
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();
}
}
}
uint16_t spi_read_ticks() {
uint8_t data[2];
WORD raw_data = enc_readReg((WORD)(0x3FFF));
data[1] = raw_data.b[1] & (0x3F);
data[0] = raw_data.b[0];
uint16_t full_data = ((uint16_t)data[1] << 8) | data[0];
return full_data;
}
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 VendorRequests(void) {
WORD32 address;
WORD result;
switch (USB_setup.bRequest) {
case TOGGLE_LED1:
led_toggle(&led1);
BD[EP0IN].bytecount = 0; // set EP0 IN byte count to 0
BD[EP0IN].status = 0xC8; // send packet as DATA1, set UOWN bit
break;
case TOGGLE_LED2:
led_toggle(&led2);
BD[EP0IN].bytecount = 0; // set EP0 IN byte count to 0
BD[EP0IN].status = 0xC8; // send packet as DATA1, set UOWN bit
break;
case READ_SW1:
BD[EP0IN].address[0] = (uint8_t)sw_read(&sw1);
BD[EP0IN].bytecount = 1; // set EP0 IN byte count to 1
BD[EP0IN].status = 0xC8; // send packet as DATA1, set UOWN bit
break;
// case ENC_WRITE_REG:
// enc_writeReg(USB_setup.wValue, USB_setup.wIndex);
// BD[EP0IN].bytecount = 0; // set EP0 IN byte count to 0
// BD[EP0IN].status = 0xC8; // send packet as DATA1, set UOWN bit
// break;
case ENC_READ_REG:
result = enc_readReg(USB_setup.wValue);
BD[EP0IN].address[0] = result.b[0];
BD[EP0IN].address[1] = result.b[1];
BD[EP0IN].bytecount = 2; // set EP0 IN byte count to 1
BD[EP0IN].status = 0xC8; // send packet as DATA1, set UOWN bit
break;
case TOGGLE_LED3:
led_toggle(&led3);
BD[EP0IN].bytecount = 0; // set EP0 IN byte count to 0
BD[EP0IN].status = 0xC8; // send packet as DATA1, set UOWN bit
break;
case READ_SW2:
BD[EP0IN].address[0] = (uint8_t)sw_read(&sw2);
BD[EP0IN].bytecount = 1; // set EP0 IN byte count to 1
BD[EP0IN].status = 0xC8; // send packet as DATA1, set UOWN bit
break;
case READ_SW3:
BD[EP0IN].address[0] = (uint8_t)sw_read(&sw3);
BD[EP0IN].bytecount = 1; // set EP0 IN byte count to 1
BD[EP0IN].status = 0xC8; // send packet as DATA1, set UOWN bit
break;
default:
USB_error_flags |= 0x01; // set Request Error Flag
}
}
uint16_t spi_read_ticks() {
uint8_t data[2];
WORD raw_data = enc_readReg((WORD)(0x3FFF));
// printf("%d\r\n", raw_data);
// printf("UNMASKED DATA: %X.%X \r\n", raw_data.b[1], raw_data.b[0]);
data[1] = raw_data.b[1] & (0x3F);
data[0] = raw_data.b[0];
// printf("MASKED DATA: %X.%X \r\n", data[1], data[0]);
uint16_t full_data = ((uint16_t)data[1] << 8) | data[0];
// printf("ENCODER TICKS: %d\r\n", full_data);
return full_data;
}
void VendorRequests(void) {
WORD32 address;
WORD result;
WORD temp;
WORD temp0, temp1;
float move_degs;
switch (USB_setup.bRequest) {
WORD temp;
case TOGGLE_LED1:
led_toggle(&led1);
BD[EP0IN].bytecount = 0; // set EP0 IN byte count to 0
BD[EP0IN].status = 0xC8; // send packet as DATA1, set UOWN bit
break;
case TOGGLE_LED2:
led_toggle(&led2);
BD[EP0IN].bytecount = 0; // set EP0 IN byte count to 0
BD[EP0IN].status = 0xC8; // send packet as DATA1, set UOWN bit
break;
case TOGGLE_LED3:
led_toggle(&led3);
BD[EP0IN].bytecount = 0; // set EP0 IN byte count to 0
BD[EP0IN].status = 0xC8; // send packet as DATA1, set UOWN bit
break;
case READ_SW1:
BD[EP0IN].address[0] = (uint8_t)sw_read(&sw1);
BD[EP0IN].bytecount = 1; // set EP0 IN byte count to 1
BD[EP0IN].status = 0xC8; // send packet as DATA1, set UOWN bit
break;
case READ_SW2:
BD[EP0IN].address[0] = (uint8_t)sw_read(&sw2);
BD[EP0IN].bytecount = 1; // set EP0 IN byte count to 1
BD[EP0IN].status = 0xC8; // send packet as DATA1, set UOWN bit
break;
case READ_SW3:
BD[EP0IN].address[0] = (uint8_t)sw_read(&sw3);
BD[EP0IN].bytecount = 1; // set EP0 IN byte count to 1
BD[EP0IN].status = 0xC8; // send packet as DATA1, set UOWN bit
break;
case ENC_READ_REG:
result = enc_readReg(USB_setup.wValue);
BD[EP0IN].address[0] = result.b[0];
BD[EP0IN].address[1] = result.b[1];
BD[EP0IN].bytecount = 2; // set EP0 IN byte count to 1
BD[EP0IN].status = 0xC8; // send packet as DATA1, set UOWN bit
break;
case SET_DAMPER_COEF:
VEL_SCALER = (float)(USB_setup.wValue.w)/(float)(USB_setup.wIndex.w);
printf("Set VEL_SCALER to %4f\r\n", VEL_SCALER);
BD[EP0IN].bytecount = 0; // set EP0 IN byte count to 0
BD[EP0IN].status = 0xC8; // send packet as DATA1, set UOWN bit
break;
case READ_DAMPER_VEL:
temp0.w = round((delta_degs + 50) * 100);
BD[EP0IN].address[0] = temp0.b[0];
BD[EP0IN].address[1] = temp0.b[1];
float step1 = (current_duty_cycle * (float)((pwm_direction == 1) ? 1:-1));
temp1.w = round( (step1 + 5) * 1000);
BD[EP0IN].address[2] = temp1.b[0];
BD[EP0IN].address[3] = temp1.b[1];
BD[EP0IN].bytecount = 4; // set EP0 IN byte count to 2
BD[EP0IN].status = 0xC8; // send packet as DATA1, set UOWN bit
break;
// case READ_VELOCITY:
// move_degs=(delta_degs + 50) * 100;
// temp.w = round(move_degs);
// BD[EP0IN].address[0] = temp.b[0];
// BD[EP0IN].address[1] = temp.b[1];
// BD[EP0IN].bytecount = 2; // set EP0 IN byte count to 2
// BD[EP0IN].status = 0xC8; // send packet as DATA1, set UOWN bit
// break;
// case READ_POSITION:
// move_degs=(current_degs + 50) * 100;
// temp.w = round(move_degs);
// BD[EP0IN].address[0] = temp.b[0];
// BD[EP0IN].address[1] = temp.b[1];
// BD[EP0IN].bytecount = 2; // set EP0 IN byte count to 2
// BD[EP0IN].status = 0xC8; // send packet as DATA1, set UOWN bit
// break;
default:
USB_error_flags |= 0x01; // set Request Error Flag
}
}
void VendorRequests(void) {
WORD32 address;
WORD result;
WORD temp;
uint16_t temp0, temp1;
float move_degs;
switch (USB_setup.bRequest) {
WORD temp;
case TOGGLE_LED1:
led_toggle(&led1);
BD[EP0IN].bytecount = 0; // set EP0 IN byte count to 0
BD[EP0IN].status = 0xC8; // send packet as DATA1, set UOWN bit
break;
case TOGGLE_LED2:
led_toggle(&led2);
BD[EP0IN].bytecount = 0; // set EP0 IN byte count to 0
BD[EP0IN].status = 0xC8; // send packet as DATA1, set UOWN bit
break;
case TOGGLE_LED3:
led_toggle(&led3);
BD[EP0IN].bytecount = 0; // set EP0 IN byte count to 0
BD[EP0IN].status = 0xC8; // send packet as DATA1, set UOWN bit
break;
case READ_SW1:
BD[EP0IN].address[0] = (uint8_t)sw_read(&sw1);
BD[EP0IN].bytecount = 1; // set EP0 IN byte count to 1
BD[EP0IN].status = 0xC8; // send packet as DATA1, set UOWN bit
break;
case READ_SW2:
BD[EP0IN].address[0] = (uint8_t)sw_read(&sw2);
BD[EP0IN].bytecount = 1; // set EP0 IN byte count to 1
BD[EP0IN].status = 0xC8; // send packet as DATA1, set UOWN bit
break;
case READ_SW3:
BD[EP0IN].address[0] = (uint8_t)sw_read(&sw3);
BD[EP0IN].bytecount = 1; // set EP0 IN byte count to 1
BD[EP0IN].status = 0xC8; // send packet as DATA1, set UOWN bit
break;
case ENC_READ_REG:
result = enc_readReg(USB_setup.wValue);
BD[EP0IN].address[0] = result.b[0];
BD[EP0IN].address[1] = result.b[1];
BD[EP0IN].bytecount = 2; // set EP0 IN byte count to 1
BD[EP0IN].status = 0xC8; // send packet as DATA1, set UOWN bit
break;
case SET_PID_P:
cur_control.Kp = (float)(USB_setup.wValue.w)/(float)(USB_setup.wIndex.w);
printf("Set Kp to %4f\r\n", cur_control.Kp);
BD[EP0IN].bytecount = 0; // set EP0 IN byte count to 0
BD[EP0IN].status = 0xC8; // send packet as DATA1, set UOWN bit
break;
case SET_PID_I:
cur_control.Ki = (float)(USB_setup.wValue.w)/(float)(USB_setup.wIndex.w);
printf("Set Ki to %4f\r\n", cur_control.Kp);
BD[EP0IN].bytecount = 0; // set EP0 IN byte count to 0
BD[EP0IN].status = 0xC8; // send packet as DATA1, set UOWN bit
break;
case SET_PID_D:
cur_control.Kd = (float)(USB_setup.wValue.w)/(float)(USB_setup.wIndex.w);
printf("Set Kd to %4f\r\n", cur_control.Kp);
BD[EP0IN].bytecount = 0; // set EP0 IN byte count to 0
BD[EP0IN].status = 0xC8; // send packet as DATA1, set UOWN bit
break;
case SET_SPRING_CONSTANT:
SPRING_CONSTANT = (float)(USB_setup.wValue.w)/(float)(USB_setup.wIndex.w);
printf("Set spring constant to %4f\r\n", SPRING_CONSTANT);
BD[EP0IN].bytecount = 0; // set EP0 IN byte count to 0
BD[EP0IN].status = 0xC8; // send packet as DATA1, set UOWN bit
break;
case READ_POSITION:
move_degs=(degs + 50) * 100;
temp.w = round(move_degs);
BD[EP0IN].address[0] = temp.b[0];
BD[EP0IN].address[1] = temp.b[1];
BD[EP0IN].bytecount = 2; // set EP0 IN byte count to 2
BD[EP0IN].status = 0xC8; // send packet as DATA1, set UOWN bit
break;
default:
USB_error_flags |= 0x01; // set Request Error Flag
}
}
