void set_data_bit(unsigned int x){
delay(1);
pin_set(port, shift, 0); //make clock low
delay(1);
pin_set(port, data, x); //data pin to x
move_bit();
}
void shift_all_bits(){
delay(1);
pin_set(port, store, 1); //clock low
delay(1);
pin_set(port, store, 0); //clock low
delay(1);
}
void move_bit(){
delay(1);
pin_set(port, shift, 1);
delay(1);
pin_set(port, shift, 0);
delay(1);
}
// Ramps up the 1.8V (directly on = unstable behaviour)
static void oled_vramp(void) {
const uint32_t count = 2000; // Sony says minimum 500
for(uint32_t outer = 0; outer < count; outer++) {
pin_set(OLED_VEN);
for(uint32_t inner = 0; inner < count; inner++) {
if(inner == outer) pin_clear(OLED_VEN);
}
}
pin_set(OLED_VEN);
}
static uint8_t __spiWriteMOSI(const SPI_SW* spi, uint8_t data){
if(spi->_bus_.order == SPI_DATA_ORDER_MSB){
// MSB is sent first
pin_set(spi->MOSI, (data & 0x80));
data<<=1;
}else{
// LSB is sent first
pin_set(spi->MOSI, (data & 1));
data>>=1;
}
return data;
}
// Sends a command to OLED
static void oled_cmd(uint8_t cmd, uint32_t count, uint8_t *data) {
pin_clear(OLED_NCS);
pin_clear(OLED_A0);
SPI_I2S_SendData(OLED_SPI, cmd);
oled_wait_spi();
pin_set(OLED_A0);
if(count) {
oled_dma(data, count);
oled_wait_dma();
oled_wait_spi();
}
pin_set(OLED_NCS);
}
void init_shift_reg(int p, int da, int shi, int str){
port = p;
data = da;
shift = shi;
store = str;
pin_configure_as_output(port, shift); //shift
pin_configure_as_output(port, store); //store
pin_configure_as_output(port, data); //data
pin_set(port, shift, 0); //default value
pin_set(port, store, 0);
}
// Blit a sprite to screen
void oled_blit(uint8_t x, uint8_t y, uint8_t w, uint8_t h, void *buffer) {
oled_window(x, y, x + w - 1, y + h - 1);
pin_clear(OLED_NCS);
pin_clear(OLED_A0);
SPI_I2S_SendData(OLED_SPI, 0xC);
oled_wait_spi();
pin_set(OLED_A0);
oled_dma((uint8_t*)buffer, (w * h) << 1);
oled_wait_dma();
oled_wait_spi();
pin_set(OLED_NCS);
}
/*!
* This function performs the branching test. It reads the current opcode and
* the output from the first source register, and then writes either BRANCH or
* NOBRANCH to the branch pin.
*/
void branch_test(BranchUnit *bru) {
int A;
int aluop;
A = pin_read(bru->in1);
aluop = pin_read(bru->op);
// If aluop is BNZ, and A is nonzero, set bru->branch to BRANCH
if (aluop == ALUOP_BNZ && A) {
pin_set(bru->branch, BRANCH);
}
// Otherwise NOBRANCH
else {
pin_set(bru->branch, NOBRANCH);
}
}
// this function turns an LED on or off.
static
void
led_set(enum led n, int on)
{
assert (n >= 0 && n < led_max);
#if ! STICK_GUEST
#if PICTOCRYPT
// red LED workaround; tri-state when off!
if (on) {
MCF_GPIO_DDRTC = 1 << n;
} else {
MCF_GPIO_DDRTC = 0;
}
MCF_GPIO_SETTC = (uint8)~(1 << n);
if (on) {
MCF_GPIO_CLRTC = (uint8)~(1 << n);
} else {
MCF_GPIO_SETTC = (uint8)(1 << n);
}
#else
pin_set(pin_assignments[pin_assignment_heartbeat], pin_type_digital_output, 0, on);
#endif // PICTOCRYPT
#endif // ! STICK_GUEST
}
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
}
}
void ds_pin_set( unsigned char x ){
pin_configure_as_output(PORT, PIN);
if( x ){
// pin_set(PORT, PIN, 0);
//do nothing, let the resistor do his work.
pin_set(PORT, PIN, 1);
//pin_configure_as_input(PORT, PIN);
// pin_configure_as_input(PORT, PIN);
} else {
pin_set(PORT, PIN, 0);
//pin_configure_as_input(PORT, PIN);
}
}
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
}
}
WORD enc_readReg(WORD address) {
WORD cmd, result;
cmd.w = 0x4000|address.w; //set 2nd MSB to 1 for a read
cmd.w |= parity(cmd.w)<<15; //calculate even parity for
pin_clear(ENC_NCS);
spi_transfer(&spi1, cmd.b[1]);
spi_transfer(&spi1, cmd.b[0]);
pin_set(ENC_NCS);
pin_clear(ENC_NCS);
result.b[1] = spi_transfer(&spi1, 0);
result.b[0] = spi_transfer(&spi1, 0);
pin_set(ENC_NCS);
return result;
}
static void setConnected(STEPPER_MOTOR* stepper, boolean connected){
L297* motor = (L297*)stepper;
// Set enable pin low to disconnect
pin_set(motor->enable, connected);
}
static void setConnected(STEPPER_MOTOR* stepper, boolean connected){
POLOLU_A4983* motor = (POLOLU_A4983*)stepper;
// Set enable pin high to disconnect
pin_set(motor->enable, (connected) ? FALSE : TRUE );
}
static void red_led(int on) {
if (on) {
pin_set(pin_e1);
} else {
pin_reset(pin_e1);
}
}
/** Deselect a device, de-asserting its chip select pin. */
static inline void spi_device_deselect(struct spi_device *dev)
{
if (dev->chip_select_active)
pin_reset(dev->chip_select);
else
pin_set(dev->chip_select);
}
static void green_led(int on) {
if (on) {
pin_set(pin_e0);
} else {
pin_reset(pin_e0);
}
}
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;
}
}
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 as5048_getRawAngle(_AS5048 *self)
{
uint8_t MSB;
uint8_t LSB;
pin_set(self->CS);
MSB=spi_transfer(&spi1,0xFF);
LSB=spi_transfer(&spi1,0xFF);
pin_clear(self->CS);
pin_set(self->CS);
MSB=spi_transfer(&spi1,0x00);
LSB=spi_transfer(&spi1,0x00);
pin_clear(self->CS);
MSB=(uint16_t)MSB;
MSB=MSB<<8;
MSB|=(uint16_t)LSB;
self->RawAngle=MSB;
}
unsigned char accel_read(unsigned char address) {
unsigned char result;
pin_clear(&ACCEL_CS);
spi_transfer(&spi1, (address&0x3F)<<1);
result = spi_transfer(&spi1, 0x00);
pin_set(&ACCEL_CS);
return result;
}
unsigned char gyro_read(unsigned char address) {
unsigned char result;
pin_clear(&GYRO_CS);
spi_transfer(&spi1, 0x80|(address&0x3F));
result = spi_transfer(&spi1, 0x00);
pin_set(&GYRO_CS);
return result;
}
void segled_set(SEGLED* led, SEGLED_SEGMENT segment, boolean value){
init(led);
if(!led->activeHigh){
value = (value) ? FALSE : TRUE;
}
const IOPin* pin = led->segment[segment];
pin_set(pin,value);
}
// Pushes n pixels of specific color (to the window)
void oled_push(uint16_t pixel, uint16_t count) {
pin_clear(OLED_NCS);
pin_clear(OLED_A0);
SPI_I2S_SendData(OLED_SPI, 0xC);
oled_wait_spi();
pin_set(OLED_A0);
pixel = ntohs(pixel); // Convert to big-endian
DMA2_Stream3->CR = repeat_cr; // Have DMA repeat word over and over
DMA2_Stream3->M0AR = (uint32_t)&pixel;
DMA2_Stream3->NDTR = count * 2;
DMA_Cmd(DMA2_Stream3, ENABLE); // Begin transfer
SPI_I2S_DMACmd(OLED_SPI, SPI_I2S_DMAReq_Tx, ENABLE);
oled_wait_dma(); // Wait for transfer to finish
oled_wait_spi();
DMA2_Stream3->CR = stream_cr; // Restore streaming DMA
pin_set(OLED_NCS);
}
/**
* Handle an IRQ for the given SPI bus.
*
* This is actually quite tricky---be very careful making changes to
* this ISR.
*
* It's important to note here that both RXNE and TXE can be set in
* the status register in the same interrupt. This can happen when
* another task disables interrupts during a transfer.
*
* To make sure that we never drop any received bytes, we always
* alternate between TXing and RXing by toggling the TX/RX interrupts
* as they occur.
*
* This makes us slightly less than optimal, as we could load the
* second byte into the TX register before we've RX'ed the first byte,
* but the additional complexity doesn't warrant complicating this ISR
* any further until we need the extra performance.
*/
static void spi_irq(struct spi_bus *bus)
{
portBASE_TYPE should_yield = pdFALSE;
uint32_t sr = bus->dev->SR;
#ifdef DEBUG_LED
pin_set(DEBUG_LED);
#endif
if (sr & SPI_SR_RXNE) {
*bus->transfer.rx_buf = bus->dev->DR;
++bus->transfer.rx_buf;
--bus->transfer.rx_len;
if (bus->transfer.rx_len == 0) {
spi_cr2_clear(bus, SPI_CR2_RXNEIE);
xSemaphoreGiveFromISR(bus->complete, &should_yield);
} else {
spi_cr2_set(bus, SPI_CR2_TXEIE);
}
} else if (sr & SPI_SR_TXE) {
spi_cr2_clear(bus, SPI_CR2_TXEIE);
if (bus->transfer.tx_len != 0) {
spi_cr2_set(bus, SPI_CR2_RXNEIE);
bus->dev->DR = *bus->transfer.tx_buf;
++bus->transfer.tx_buf;
--bus->transfer.tx_len;
}
}
if (sr & SPI_ERROR_BITS) {
#ifdef ERROR_LED
pin_set(ERROR_LED);
#endif
}
#ifdef DEBUG_LED
pin_reset(DEBUG_LED);
#endif
if (should_yield == pdTRUE)
taskYIELD();
}
void showBlank(void){
volatile uint8_t segments = 0b00000000;
volatile uint8_t segmentsZero = 0b00000000;
int a;
for(a = 0; a < 3; a++){
int z;
for (z = 0 ; z < 8 ; z++){
volatile uint8_t transferSegment = segments & (1 << (7 - z));
pin_clear(segmentClock);
pin_write(segmentData, transferSegment);
pin_set(segmentClock);
}
}
pin_clear(segmentLatch);
pin_set(segmentLatch);
}
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);
}
}
}
static void writeNibble(const HD44780* device, const uint8_t data, uint8_t pos){
uint8_t mask = 1;
for(uint8_t i=0; i<4; i++){
boolean val = (data & mask) ? TRUE : FALSE;
pin_set(device->data[pos++],val);
mask <<= 1;
}
}
