int main(){
int fd;
int i;
if ((fd = open("/dev/mem", O_RDWR | O_SYNC))==-1){
printf("Could not open /dev/mem\n");
return 0;
}
printf("opened /dev/mem\n");
pwm_map_base[0]= mmap(0,getpagesize(),PROT_READ|PROT_WRITE,MAP_SHARED,fd,PWM0_BASE);
pwm_map_base[1]= mmap(0,getpagesize(),PROT_READ|PROT_WRITE,MAP_SHARED,fd,PWM1_BASE);
pwm_map_base[2]= mmap(0,getpagesize(),PROT_READ|PROT_WRITE,MAP_SHARED,fd,PWM2_BASE);
if(pwm_map_base[0] == (void *) -1) {
printf("Unable to mmap pwm\n");
exit(-1);
}
close(fd);
//turn off clock to eqep
*(unsigned long*)(pwm_map_base[0]+PWMSS_CLKCONFIG) &= ~0x010;
// Write the decoder control settings
*(unsigned short*)(pwm_map_base[0]+EQEP_OFFSET+QDECCTL) = 0;
// set maximum position to two's compliment of -1, aka UINT_MAX
*(unsigned long*)(pwm_map_base[0]+EQEP_OFFSET+QPOSMAX)=-1;
// Enable interrupt
*(unsigned short*)(pwm_map_base[0]+EQEP_OFFSET+QEINT) = UTOF;
// set unit period register
*(unsigned long*)(pwm_map_base[0]+EQEP_OFFSET+QUPRD)=0x5F5E100;
// enable counter in control register
*(unsigned short*)(pwm_map_base[0]+EQEP_OFFSET+QEPCTL) = PHEN|IEL0|SWI|UTE|QCLM;
//enable clock from PWMSS
*(unsigned long*)(pwm_map_base[0]+PWMSS_CLKCONFIG) |= 0x010;
// PWMSS registers
printf("SYSCONFIG 0x%lX\n", *(unsigned long*)(pwm_map_base[0]+PWMSS_SYSCONFIG));
printf("CLKCONFIG 0x%lX\n", *(unsigned long*)(pwm_map_base[0]+PWMSS_CLKCONFIG));
// eQep registers
printf("QPOSMAX0 0x%lX\n", *(unsigned long*)(pwm_map_base[0]+EQEP_OFFSET+QPOSMAX));
printf("QEPCTL0 0x%X\n", *(unsigned short*)(pwm_map_base[0]+EQEP_OFFSET+QEPCTL));
printf("QDECCTL0 0x%X\n", *(unsigned short*)(pwm_map_base[0]+EQEP_OFFSET+QDECCTL));
printf("QEINT0 0x%X\n", *(unsigned short*)(pwm_map_base[0]+EQEP_OFFSET+QEINT));
printf("QUPRD0 0x%lX\n", *(unsigned long*)(pwm_map_base[0]+EQEP_OFFSET+QUPRD));
printf("QUTMR0 0x%lX\n", *(unsigned long*)(pwm_map_base[0]+EQEP_OFFSET+QUTMR));
printf("QEPSTS0 0x%X\n", *(unsigned short*)(pwm_map_base[0]+EQEP_OFFSET+QEPSTS));
printf("\n");
set_encoder_pos(0,0);
set_encoder_pos(1,0);
set_encoder_pos(2,0);
// print out current eQep position for a while
for(i=0;i<100000;i++){
printf("\reqep0: %ld eqep1: %ld eqep2: %ld ",get_encoder_pos(0),get_encoder_pos(1),get_encoder_pos(2));
fflush(stdout);
usleep(10000);
}
return 0;
}
/******************************************************************
* zero_out_controller()
* clear the controller state and setpoint
* especially should be called before swapping state to RUNNING
* keep current theta and vbatt since they may be used by
* other threads
*******************************************************************/
int zero_out_controller(){
// store theta and vbatt
float theta_tmp = cstate.current_theta;
float vBatt_tmp = cstate.vBatt;
// wipe state
memset(&cstate, 0, sizeof(core_state_t));
// restore theta and vbatt
cstate.current_theta = theta_tmp;
cstate.vBatt = vBatt_tmp;
// zero out encoder counters
set_encoder_pos(config.encoder_channel_L,0);
set_encoder_pos(config.encoder_channel_R,0);
// store previous controller mode
core_mode_t last_mode = setpoint.core_mode;
// wipe setpoint
memset(&setpoint, 0, sizeof(core_setpoint_t));
setpoint.core_mode = last_mode;
return 0;
}
int initialize_cape(){
FILE *fd; // opened and closed for each file
char path[128]; // buffer to store file path string
int i = 0; // general use counter
printf("\n");
// check if another project was using resources
// kill that process cleanly with sigint if so
fd = fopen(LOCKFILE, "r");
if (fd != NULL) {
int old_pid;
fscanf(fd,"%d", &old_pid);
if(old_pid != 0){
printf("warning, shutting down existing robotics project\n");
kill((pid_t)old_pid, SIGINT);
sleep(1);
}
// close and delete the old file
fclose(fd);
remove(LOCKFILE);
}
// create new lock file with process id
fd = fopen(LOCKFILE, "ab+");
if (fd < 0) {
printf("\n error opening LOCKFILE for writing\n");
return -1;
}
pid_t current_pid = getpid();
printf("Current Process ID: %d\n", (int)current_pid);
fprintf(fd,"%d",(int)current_pid);
fflush(fd);
fclose(fd);
// ensure gpios are exported
printf("Initializing GPIO\n");
for(i=0; i<NUM_OUT_PINS; i++){
if(gpio_export(out_gpio_pins[i])){
printf("failed to export gpio %d", out_gpio_pins[i]);
return -1;
};
gpio_set_dir(out_gpio_pins[i], OUTPUT_PIN);
}
// set up default values for some gpio
disable_motors();
deselect_spi1_slave(1);
deselect_spi1_slave(2);
//Set up PWM
printf("Initializing PWM\n");
i=0;
for(i=0; i<4; i++){
strcpy(path, pwm_files[i]);
strcat(path, "polarity");
fd = fopen(path, "a");
if(fd<0){
printf("PWM polarity not available in /sys/class/devices/ocp.3\n");
return -1;
}
//set correct polarity such that 'duty' is time spent HIGH
fprintf(fd,"%c",'0');
fflush(fd);
fclose(fd);
}
//leave duty cycle file open for future writes
for(i=0; i<4; i++){
strcpy(path, pwm_files[i]);
strcat(path, "duty");
pwm_duty_pointers[i] = fopen(path, "a");
}
//read in the pwm period defined in device tree overlay .dts
strcpy(path, pwm_files[0]);
strcat(path, "period");
fd = fopen(path, "r");
if(fd<0){
printf("PWM period not available in /sys/class/devices/ocp.3\n");
return -1;
}
fscanf(fd,"%i", &pwm_period_ns);
fclose(fd);
// mmap pwm modules to get fast access to eQep encoder position
// see mmap_eqep example program for more mmap and encoder info
printf("Initializing eQep Encoders\n");
int dev_mem;
if ((dev_mem = open("/dev/mem", O_RDWR | O_SYNC))==-1){
printf("Could not open /dev/mem \n");
return -1;
}
pwm_map_base[0] = mmap(0,getpagesize(),PROT_READ|PROT_WRITE,MAP_SHARED,dev_mem,PWM0_BASE);
pwm_map_base[1] = mmap(0,getpagesize(),PROT_READ|PROT_WRITE,MAP_SHARED,dev_mem,PWM1_BASE);
pwm_map_base[2] = mmap(0,getpagesize(),PROT_READ|PROT_WRITE,MAP_SHARED,dev_mem,PWM2_BASE);
if(pwm_map_base[0] == (void *) -1) {
printf("Unable to mmap pwm \n");
return(-1);
}
close(dev_mem);
// Test eqep and reset position
for(i=1;i<3;i++){
if(set_encoder_pos(i,0)){
printf("failed to access eQep register\n");
printf("eQep driver not loaded\n");
return -1;
}
}
//set up function pointers for button press events
printf("starting button interrupts\n");
set_pause_pressed_func(&null_func);
set_pause_unpressed_func(&null_func);
set_mode_pressed_func(&null_func);
set_mode_unpressed_func(&null_func);
initialize_button_handlers();
// Load binary into PRU
printf("Starting PRU servo controller\n");
if(initialize_pru_servos()){
printf("WARNING: PRU init FAILED");
}
// Print current battery voltage
printf("Battery Voltage = %fV\n", getBattVoltage());
// Start Signal Handler
printf("Enabling exit signal handler\n");
signal(SIGINT, ctrl_c);
// all done
set_state(PAUSED);
printf("\nRobotics Cape Initialized\n");
return 0;
}
int initialize_cape(){
FILE *fd; // opened and closed for each file
char path[MAX_BUF]; // buffer to store file path string
int i = 0; // general use counter
printf("\nInitializing GPIO\n");
for(i=0; i<NUM_OUT_PINS; i++){
gpio_export(out_gpio_pins[i]);
gpio_set_dir(out_gpio_pins[i], OUTPUT_PIN);
}
//Set up PWM
printf("Initializing PWM\n");
i=0;
for(i=0; i<6; i++){
strcpy(path, pwm_files[i]);
strcat(path, "polarity");
fd = fopen(path, "a");
if(fd<0){
printf("PWM polarity not available in /sys/class/devices/ocp.3\n");
return -1;
}
//set correct polarity such that 'duty' is time spent HIGH
fprintf(fd,"%c",'0');
fflush(fd);
fclose(fd);
}
//leave duty cycle file open for future writes
for(i=0; i<6; i++){
strcpy(path, pwm_files[i]);
strcat(path, "duty");
pwm_duty_pointers[i] = fopen(path, "a");
}
//read in the pwm period defined in device tree overlay .dts
strcpy(path, pwm_files[0]);
strcat(path, "period");
fd = fopen(path, "r");
if(fd<0){
printf("PWM period not available in /sys/class/devices/ocp.3\n");
return -1;
}
fscanf(fd,"%i", &pwm_period_ns);
fclose(fd);
// mmap pwm modules to get fast access to eQep encoder position
// see mmap_eqep example program for more mmap and encoder info
printf("Initializing eQep Encoders\n");
int dev_mem;
if ((dev_mem = open("/dev/mem", O_RDWR | O_SYNC))==-1){
printf("Could not open /dev/mem \n");
return -1;
}
pwm_map_base[0] = mmap(0,getpagesize(),PROT_READ|PROT_WRITE,MAP_SHARED,dev_mem,PWM0_BASE);
pwm_map_base[1] = mmap(0,getpagesize(),PROT_READ|PROT_WRITE,MAP_SHARED,dev_mem,PWM1_BASE);
pwm_map_base[2] = mmap(0,getpagesize(),PROT_READ|PROT_WRITE,MAP_SHARED,dev_mem,PWM2_BASE);
if(pwm_map_base[0] == (void *) -1) {
printf("Unable to mmap pwm \n");
return(-1);
}
close(dev_mem);
// Test eqep and reset position
for(i=1;i<3;i++){
if(set_encoder_pos(i,0)){
printf("failed to access eQep register\n");
printf("eQep driver not loaded\n");
return -1;
}
}
//set up function pointers for button press events
set_start_pressed_func(&null_func);
set_start_unpressed_func(&null_func);
set_select_pressed_func(&null_func);
set_select_unpressed_func(&null_func);
//event handler thread for buttons
printf("Starting Event Handler\n");
pthread_t event_thread;
pthread_create(&event_thread, NULL, read_events, (void*) NULL);
// Print current battery voltage
printf("Battery Voltage = %fV\n", getBattVoltage());
// Start Signal Handler
printf("Enabling exit signal handler\n");
signal(SIGINT, ctrl_c);
// all done
set_state(PAUSED);
printf("\nRobotics Cape Initialized\n");
return 0;
}
