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;
}
int main(){
initialize_cape();
setGRN(HIGH);
setRED(HIGH);
printf("\n\nRaw data for encoders 1,2,3\n");
while(get_state() != EXITING){
printf("\r%3li %3li %3li ", get_encoder_pos(1),get_encoder_pos(2),get_encoder_pos(3));
fflush(stdout);
usleep(50000);
}
cleanup_cape();
return 0;
}
int main(int argc, char *argv[])
{
unsigned int *pru; // Points to start of PRU memory.
int fd;
printf("Encoder tester\n");
fd = open ("/dev/mem", O_RDWR | O_SYNC);
if (fd == -1) {
printf ("ERROR: could not open /dev/mem.\n\n");
return 1;
}
pru = mmap (0, PRU_LEN, PROT_READ | PROT_WRITE, MAP_SHARED, fd, PRU_ADDR);
if (pru == MAP_FAILED) {
printf ("ERROR: could not map memory.\n\n");
return 1;
}
close(fd);
printf ("Using /dev/mem.\n");
prusharedMem_32int_ptr = pru + PRU_SHAREDMEM/4; // Points to start of shared memory
printf("\nRaw encoder positions\n");
printf(" E4 |");
printf(" \n");
int i;
while(1){
printf("\r");
for(i=4;i<=4;i++){
printf("%6d |", get_encoder_pos(i));
}
fflush(stdout);
usleep(50000);
}
if(munmap(pru, PRU_LEN)) {
printf("munmap failed\n");
} else {
printf("munmap succeeded\n");
}
}
/******************************************************************
* balance_core()
* discrete-time balance controller operated off IMU interrupt
* Called at SAMPLE_RATE_HZ
*******************************************************************/
int balance_core(){
// local variables only in memory scope of balance_core
static int D1_saturation_counter = 0;
float compensated_D1_output = 0;
float dutyL = 0;
float dutyR = 0;
static log_entry_t new_log_entry;
float output_scale; //battery voltage/nominal voltage
// if an IMU packet read failed, ignore and just return
// the mpu9150_read function may print it's own warnings
if (mpu9150_read(&mpu) != 0){
return -1;
}
/**************************************************************
* STATE_ESTIMATION
* read sensors and compute the state regardless of if the
* controller is ARMED or DISARMED
***************************************************************/
// angle theta is positive in the direction of forward tip
// add mounting angle of BBB
cstate.theta[2] = cstate.theta[1]; cstate.theta[1] = cstate.theta[0];
cstate.theta[0] = mpu.fusedEuler[VEC3_X] + config.bb_mount_angle;
cstate.current_theta = cstate.theta[0];
cstate.d_theta = (cstate.theta[0]-cstate.theta[1])/DT;
// collect encoder positions
cstate.wheelAngleR = -(get_encoder_pos(config.encoder_channel_R) * 2*PI) \
/(config.gearbox * config.encoder_res);
cstate.wheelAngleL = (get_encoder_pos(config.encoder_channel_L) * 2*PI) \
/(config.gearbox * config.encoder_res);
// log phi estimate
// wheel angle is relative to body,
// add theta body angle to get absolute wheel position
cstate.phi[2] = cstate.phi[1]; cstate.phi[1] = cstate.phi[0];
cstate.phi[0] = ((cstate.wheelAngleL + cstate.wheelAngleR)/2) +cstate.current_theta;
cstate.current_phi = cstate.phi[0];
cstate.d_phi = (cstate.phi[0]-cstate.phi[1])/DT;
// body turning estimation
cstate.gamma[2] = cstate.gamma[1]; cstate.gamma[1] = cstate.phi[0];
cstate.gamma[0]=(cstate.wheelAngleL-cstate.wheelAngleR) \
* (config.wheel_radius/config.track_width);
cstate.d_gamma = (cstate.gamma[0]-cstate.gamma[1])/DT;
cstate.current_gamma = cstate.gamma[0];
// output scaling
output_scale = cstate.vBatt/config.v_nominal;
/*************************************************************
* Control based on the robotics_library defined state variable
* PAUSED: make sure the controller stays DISARMED
* RUNNING: Normal operation of controller.
* - check for tipover
* - wait for MiP to be within config.start_angle of
* upright
* - choose mode from setpoint.core_mode
* - evaluate difference equation and check saturation
* - actuate motors
***************************************************************/
switch (get_state()){
// make sure things are off if program is closing
case EXITING:
disable_motors();
return 0;
// if the controller is somehow still ARMED, disarm it
case PAUSED:
if(setpoint.arm_state==ARMED){
disarm_controller();
}
break;
// normal operating mode
case RUNNING:
// exit if the controller was not armed properly
if(setpoint.arm_state==DISARMED){
return 0;
}
// check for a tipover before anything else
if(fabs(cstate.current_theta)>config.tip_angle){
disarm_controller();
printf("tip detected \n");
break;
}
/**********************************************************
* POSITION Phi controller
* feedback control of wheel angle Phi by outputting a
* reference theta body angle. This is controller D2 in
* config
***********************************************************/
if(setpoint.core_mode == POSITION){
// move the position set points based on user input
if(setpoint.phi_dot != 0.0){
//setpoint.phi == cstate.current_phi + setpoint.phi_dot*DT;
setpoint.phi += setpoint.phi_dot * DT;
}
// march the different equation terms for the input Phi Error
// and the output theta reference angle
cstate.ePhi[2] = cstate.ePhi[1];
cstate.ePhi[1] = cstate.ePhi[0];
cstate.ePhi[0] = setpoint.phi-cstate.current_phi;
cstate.theta_ref[2] = cstate.theta_ref[1];
cstate.theta_ref[1] = cstate.theta_ref[0];
// evaluate D2 difference equation
cstate.theta_ref[0] = config.K_D2*( \
config.numD2_2 * cstate.ePhi[2] \
+ config.numD2_1 * cstate.ePhi[1] \
+ config.numD2_0 * cstate.ePhi[0]) \
-(config.denD2_2 * cstate.theta_ref[2] \
+ config.denD2_1 * cstate.theta_ref[1]);
//check saturation of outer loop theta reference output signal
saturate_float(&cstate.theta_ref[0],-config.theta_ref_max,config.theta_ref_max);
setpoint.theta = cstate.theta_ref[0];
}
// evaluate inner loop controller D1z
cstate.eTheta[2] = cstate.eTheta[1];
cstate.eTheta[1] = cstate.eTheta[0];
cstate.eTheta[0] = setpoint.theta - cstate.current_theta;
cstate.u[2] = cstate.u[1];
cstate.u[1] = cstate.u[0];
cstate.u[0] = \
config.K_D1 * (config.numD1_0 * cstate.eTheta[0] \
+ config.numD1_1 * cstate.eTheta[1] \
+ config.numD1_2 * cstate.eTheta[2]) \
- (config.denD1_1 * cstate.u[1] \
+ config.denD1_2 * cstate.u[2]);
// check saturation of inner loop knowing that right after
// this control will be scaled by battery voltage
if(saturate_float(&cstate.u[0], -output_scale, output_scale)){
D1_saturation_counter ++;
if(D1_saturation_counter > SAMPLE_RATE_HZ*config.pickup_detection_time){
printf("inner loop controller saturated\n");
disarm_controller();
D1_saturation_counter = 0;
break;
}
}
else{
D1_saturation_counter = 0;
}
cstate.current_u = cstate.u[0];
// scale output to compensate for battery charge level
compensated_D1_output = cstate.u[0] / output_scale;
// // integrate the reference theta to correct for imbalance or sensor
// // only if standing relatively still with zero phi reference
// // to-do, wait for stillness for a time period before integrating
// if(setpoint.phi==0 && fabs(cstate.phi_dot)<2){
// state.thetaTrim += (config.kTrim*cstate.theta_ref[0]) * DT;
// }
//steering controller
// move the controller set points based on user input
setpoint.gamma += setpoint.gamma_dot * DT;
cstate.egamma[1] = cstate.egamma[0];
cstate.egamma[0] = setpoint.gamma - cstate.current_gamma;
cstate.duty_split = config.KP_steer*(cstate.egamma[0] \
+config.KD_steer*(cstate.egamma[0]-cstate.egamma[1]));
// if the steering input would saturate a motor, reduce
// the steering input to prevent compromising balance input
if(fabs(compensated_D1_output)+fabs(cstate.duty_split) > 1){
if(cstate.duty_split > 0){
cstate.duty_split = 1-fabs(compensated_D1_output);
}
else cstate.duty_split = -(1-fabs(compensated_D1_output));
}
// add D1 balance controller and steering control
dutyL = compensated_D1_output - cstate.duty_split;
dutyR = compensated_D1_output + cstate.duty_split;
// send to motors
// one motor is flipped on chassis so reverse duty to L
set_motor(config.motor_channel_L,-dutyL);
set_motor(config.motor_channel_R,dutyR);
cstate.time_us = microsSinceEpoch();
// pass new information to the log with add_to_buffer
// this only puts information in memory, doesn't
// write to disk immediately
if(config.enable_logging){
new_log_entry.time_us = cstate.time_us-core_start_time_us;
new_log_entry.theta = cstate.current_theta;
new_log_entry.theta_ref = setpoint.theta;
new_log_entry.phi = cstate.current_phi;
new_log_entry.u = cstate.current_u;
add_to_buffer(new_log_entry);
}
// end of normal balancing routine
// last_state will be updated beginning of next interrupt
break;
default:
break; // nothing to do if UNINITIALIZED
}
return 0;
}
