How to use a custom servo component in Nios2? - c++

Question

Hello my goal is to design an embedded system for the Nios II processor and DE1-SoC development board that will control a servo motor to sweep between two angles. The embedded system should contain a custom IP that creates the appropriate Pulse-Width Modulation (PWM) signal to sweep the servo-motor’s arm. The system also contains 8 switches for entering the start and stop angles, 2 pushbuttons for “locking” in the angles and 5 seven-segment displays for displaying the start and stop angles. I am experimenting with the custom servo motor component in Nios and I am not sure how to proceed. I have made the component to accept inputs from the switches, but in previous trials I could not get the servo motor to move.
Any help would be appreciated
Here is my code:
#include "sys/alt_stdio.h"
#include "io.h"
#include <stdio.h>
#include "system.h"
#include "alt_types.h"
#include "sys/alt_irq.h"
#include "altera_avalon_pio_regs.h"
#define HEXZERO 0x40
#define HEXONE 0x79
#define HEXTWO 0x24
#define HEXTHREE 0x30
#define HEXFOUR 0x19
#define HEXFIVE 0x12
#define HEXSIX 0x02
#define HEXSEVEN 0x78
#define HEXEIGHT 0x00
#define HEXNINE 0x10
//array
char hexArray0 [] = { HEXZERO, HEXONE, HEXTWO, HEXTHREE, HEXFOUR, HEXFIVE, HEXSIX, HEXSEVEN, HEXEIGHT, HEXNINE };
char hexArray1 [] = { HEXZERO, HEXONE, HEXTWO, HEXTHREE, HEXFOUR, HEXFIVE, HEXSIX, HEXSEVEN, HEXEIGHT, HEXNINE };
char hexArray2 [] = { HEXZERO, HEXONE, HEXTWO, HEXTHREE, HEXFOUR, HEXFIVE, HEXSIX, HEXSEVEN, HEXEIGHT, HEXNINE };
char hexArray4 [] = { HEXZERO, HEXONE, HEXTWO, HEXTHREE, HEXFOUR, HEXFIVE, HEXSIX, HEXSEVEN, HEXEIGHT, HEXNINE };
char hexArray5 [] = { HEXZERO, HEXONE, HEXTWO, HEXTHREE, HEXFOUR, HEXFIVE, HEXSIX, HEXSEVEN, HEXEIGHT, HEXNINE };
// create standard embedded type definitions
typedef signed char sint8; // signed 8 bit values
typedef unsigned char uint8; // unsigned 8 bit values
typedef signed short sint16; // signed 16 bit values
typedef unsigned short uint16; // unsigned 16 bit values
typedef signed long sint32; // signed 32 bit values
typedef unsigned long uint32; // unsigned 32 bit values
typedef float real32; // 32 bit real values
//set up pointers to peripherals
//uint32* TimerPtr = (uint32*)TIMER_0_BASE;
uint32* JtagUartPtr = (uint32*)JTAG_UART_0_BASE;
uint32* CustomIpPtr = (uint32*)SERVO_CONTROLLER_0_BASE;
unsigned char* switchesBase_ptr = (unsigned char*)SWITCHES_BASE;
unsigned char* hex_ptr0 = (unsigned char*)HEX0_BASE;
unsigned char* hex_ptr1 = (unsigned char*)HEX1_BASE;
unsigned char* hex_ptr2 = (unsigned char*)HEX2_BASE;
unsigned char* hex_ptr4 = (unsigned char*)HEX4_BASE;
unsigned char* hex_ptr5 = (unsigned char*)HEX5_BASE;
unsigned char* buttons_ptr = (unsigned char*)PUSHBUTTONS_BASE;
int arrayCounter=0;
unsigned int read_push=0 ;
unsigned int read_switch=0 ;
unsigned int isr_flag=0 ; //if flag=0 means miniumum angle / flag=1 means maximum angle
int servo_input_min=0;
int servo_input_max=0;
int key3,key2 =0;
int hex0_index, hex1_index, hex2_index,hex4_index,hex5_index;
int fortySix= 0b00101110; //binary
int fortySix_n= 0x0000C57A; //fortySix*1099;
int oneThreeFour= 0b10000110; //binary
int oneThreeFour_n= 0x00018464; //oneThreeSix*742;
int eighty= 0b01010000; //binary
int eighty_n=0x00010F40; // eighty*868 ;
int hundred= 0b01100100; //binary
int hundred_n= 0x00013A74; //hundred*805;
int fortyFive= 0b00101101; //binary
int fortyFive_n= 0x0000C350 ;//fortyFive*1110;
int ninety= 0b01011010; //binary
int ninety_n=0x000124F8;// ninety* 833;
int oneThreeFive=0b10000111; //binary
int oneThreeFive_n= 0x000186A0; //oneThreeFive*740;
void button_isr(void *context)
/*****************************************************************************/
/* Interrupt Service Routine */
/* Determines what caused the interrupt and calls the appropriate */
/* subroutine. */
/* */
/*****************************************************************************/
{
unsigned char current_val;
//clear timer interrupt
//*TimerPtr = 0;
current_val= *buttons_ptr;
if(current_val==0b1000){
key3=1;
}
else if(current_val==0b0100){
key2=1;
}
*(buttons_ptr + 12)= 1;
return;
}
void servo_isr(void *context){
*(CustomIpPtr ) = servo_input_min;
*(CustomIpPtr + 1) = servo_input_max;
return;
}
int main()
{
printf("Hello from Nips II!\n");
uint32 i;
servo_input_min=fortyFive_n;
servo_input_max=oneThreeFive_n;
*hex_ptr0 = hexArray0[5]; //5
*hex_ptr1 = hexArray1[3]; //3
*hex_ptr2 = hexArray2[1]; //1
*hex_ptr4 = hexArray4[5]; //5
*hex_ptr5 = hexArray5[4]; //4
alt_ic_isr_register(SERVO_CONTROLLER_0_IRQ_INTERRUPT_CONTROLLER_ID, SERVO_CONTROLLER_0_IRQ, servo_isr,CustomIpPtr,0);
*(CustomIpPtr ) = servo_input_min;
*(CustomIpPtr + 1) = servo_input_max;
alt_ic_isr_register(PUSHBUTTONS_IRQ_INTERRUPT_CONTROLLER_ID,PUSHBUTTONS_IRQ,button_isr,0,0);
*(buttons_ptr + 8)= 0x0C; //key 3,2
while (1) { //start loop
*buttons_ptr &= 0x0C;
*switchesBase_ptr &= 0xFF;
read_push = *buttons_ptr;
read_switch = *switchesBase_ptr;
if (read_switch == fortySix) {
servo_input_min = fortySix_n;
isr_flag = 0; //key3
} else if (read_switch == oneThreeFour) {
servo_input_max = oneThreeFour_n;
isr_flag = 1; //key2
} else if (read_switch == eighty) {
servo_input_min = eighty_n;
isr_flag = 0; //key3
} else if (read_switch == hundred) {
servo_input_max = hundred_n;
isr_flag = 1; //key2
} else if (read_switch == ninety) {
servo_input_min = ninety_n;
servo_input_max = ninety_n;
isr_flag = 1; //key 3,2
}
if (isr_flag == 1 && key2 == 1) { //max angle
while (read_push == 13) { //while button is still pressed stay in loop
*buttons_ptr &= 0x0C;
read_push = *buttons_ptr;
}
if (servo_input_max == oneThreeFour_n) { //134
hex2_index = 1;
hex1_index = 3;
hex0_index = 4;
} else if (servo_input_max == hundred_n) { //100
hex2_index = 1;
hex1_index = 0;
hex0_index = 0;
} else if (servo_input_max == ninety_n) { //90
hex2_index = 9;
hex1_index = 0;
hex0_index = 0;
}
*hex_ptr2 = hexArray0[hex2_index];
*hex_ptr1 = hexArray0[hex1_index];
*hex_ptr0 = hexArray0[hex0_index];
while (read_push != 15) { //while button is still unpressed stay in loop 15=0
*buttons_ptr &= 0x0C;
read_push = *buttons_ptr;
}
} else if (isr_flag == 0 && key3 == 1) { //min angle
while (read_push != 13) { //while button is still unpressed stay in loop 15=0
*buttons_ptr &= 0x0C;
read_push = *buttons_ptr;
}
if (servo_input_max == fortySix_n) { //46
hex5_index = 4;
hex4_index = 6;
} else if (servo_input_max == eighty_n) { //80
hex5_index = 8;
hex4_index = 0;
} else if (servo_input_max == ninety_n) { //90
hex5_index = 9;
hex4_index = 0;
}
*hex_ptr5 = hexArray0[hex5_index];
*hex_ptr4 = hexArray0[hex4_index];
while (read_push != 15) { //while button is still unpressed stay in loop 15=0
*buttons_ptr &= 0x0C;
read_push = *buttons_ptr;
}
}
}
return 0;
}