void UserTask() {
DISPLAY lcd;
int page=0;
lcd.drawForm(-2);
for (int i=0; i<30; i++) {
TimerWait(100);
if (EventCatch(JOY_MASK)) {
EventRaise(EVT_EXIT);
for (;;) NextTask();
}
}
lcd.drawForm(page);
for (;;) {
switch (EventCatch(JOY_MASK)) {
case JOY_UP:
if (page>0) lcd.drawForm(--page);
break;
case JOY_DOWN:
if (page<3) lcd.drawForm(++page);
break;
case JOY_PUSH:
lcd.drawForm(-1);
EventRaise(EVT_EXIT+EVT_SHUTDOWN);
for (;;) NextTask();
}
lcd.drawData(page);
NextTask();
}
}
int main (void)
{
Initialization();
show_mhz(); // Display CPU speed
TimerWait(1000); // Wait 1 second
while(1)
{
printLCD("Show ADC"); // Display the text in quotes on the LCD
while (!KEY_VALID); // Wait for joystick to be moved or pressed.
if (getkey() == 1) // If enter was pressed then do what is in the braces, just skip over it.
{
TimerWait(500); // debounce joystick button
showADC();
printLCD("Back ADCs");
TimerWait(2000);
}
printLCD("Balance"); // Display the text in quotes on the LCD
while (!KEY_VALID); // Wait for joystick to be moved or pressed.
if (getkey() == 1) // If enter was pressed then do what is in the braces, else just continue.
{
TimerWait(500); // debounce joystick button
balance();
printLCD("Back Balance");
TimerWait(2000);
}
printLCD("rprintf"); // Display the text in quotes on the LCD
while (!KEY_VALID); // Wait for joystick to be moved or pressed.
if (getkey() == 1) // If enter was pressed then do what is in the braces, else just continue.
{
TimerWait(500); // debounce joystick button
rprintf_test();
printLCD("Back rprintf");
TimerWait(2000);
}
printLCD("PWM Test"); // Display the text in quotes on the LCD
while (!KEY_VALID); // Wait for joystick to be moved or pressed.
if (getkey() == 1) // If enter was pressed then do what is in the braces, else just continue.
{
TimerWait(500); // debounce joystick button
PWM_Test();
printLCD("Back PWM Test");
TimerWait(3000);
}
}
}
/**
Stalls the CPU for the number of microseconds specified by MicroSeconds.
@param MicroSeconds The minimum number of microseconds to delay.
@return The value of MicroSeconds inputted.
**/
UINTN
EFIAPI
MicroSecondDelay (
IN UINTN MicroSeconds
)
{
TimerWait(uSecsToTB(MicroSeconds));
return MicroSeconds;
}
void test_calls_wptr_time (Workspace p)
{
int timeo, to2;
timeo = TimerRead (p);
to2 = timeo + 42;
TimerWait (p, to2);
if (Time_AFTER (to2, timeo)) {
/* boo */
StopP (p);
}
}
/*****************************************************************************
* showADC - Displays ADC reading on the LCD display.
*****************************************************************************/
void showADC(void)
{
InitADC();
while (!(getkey() == 1))
{
gyroRaw = GetADC(gyro_sensor);
accelRaw = GetADC(accel_sensor);
TimerWait(100);
show12bits(gyroRaw, accelRaw);
}
LCD_ShowColons(0);
}
void SampleLoopTask::Task(){
int cnt = 0;
int region;
int attempt = 0;
static int dout = 1;
static double error = 0;
int last_status = 0;
double pos_prev = 0;
double pCmd_prev = 0;
double vCmd_prev = 0;
int obj_ang_index = 0; // to keep continuous angle value when acrossing pi(or -pi).
uint64_t cycle, cycle_prev;
// for acceleration calculation
double u = 0;
double alpha = 0;
double DuDeta1Bar =0;
double DuDeta2 =0;
double DalphaDeta1Bar =0;
double DalphaDeta2 =0;
double eta1Bar = 0;
double z = 0;
// to use eta2D in calculation of shD
double eta2_prev = 0;
double eta2D = 0;
double eta2D_prev = 0;
double elapsedSec = 0;
//test
double feedforwardVal = 0;
uint64_t cycle1, cycle2;
while(!lost){
TimerWait();
ncycles = ClockCycles();
sec=(double)(ncycles - ncycles_prev)/cps;
ncycles_prev = ncycles;
TimingProcess();
// Read Inputs
HW->ProcessInput(); // all digital & analog, encoders reading.
// Get status of camera
last_status = HW->ReadDigitalBit(IoHardware::FRAME_STATUS);
// Send out pulse to trigger camera
HW->WriteDigitalBit(IoHardware::CAMERA_TRIGGER, 1);
HW->WriteDigitalBit(IoHardware::CAMERA_TRIGGER, 0);
// test
//cycle1 = ClockCycles();
if(camera) {// && !lost){
// Wait for camera to process data, with timeout counter
while(HW->ReadDigitalBit(IoHardware::FRAME_STATUS) == last_status){
if(++attempt == (int)(6.0e5/SAMPLE_RATE)) { // 5.0e5 must be found out by experiments to give the smallest time to determine an error status
HW->SetAnalogOut(IoHardware::AMP_SIGNAL, 0.0);
HW->WriteDigitalBit(IoHardware::MOTOR_ENABLE, MOTOR_OFF);
HW->motorStatus = MOTOR_OFF;
lost = 1;
FATAL_ERROR("Frame not received -");
//strcpy(err_msg, "Frame not received -"); // // not work this way.
}
}
attempt = 0;
vnum[0] = -99;
int n = VNET->Recv();
region = (int)vnum[0];
switch(region){
case 0: //ROI_0:
obj.x0 = vnum[1]; //mm
obj.y0 = vnum[2]; //mm
break;
case 1: //ROI_1:
obj.x1 = vnum[1]; //mm
obj.y1 = vnum[2]; //mm
break;
default:
HW->SetAnalogOut(IoHardware::AMP_SIGNAL, 0.0);
HW->WriteDigitalBit(IoHardware::MOTOR_ENABLE, MOTOR_OFF);
HW->motorStatus = MOTOR_OFF;
printf("roi-vnum[0]:%d\n", region);
FATAL_ERROR("Object lost -");
lost = 1;
}
obj.x = (obj.x0 + obj.x1)/2/1000; // convert to m
obj.y = (obj.y0 + obj.y1)/2/1000;
double obj_raw_angle = atan2(obj.y1-obj.y0, obj.x1-obj.x0); // within -pi to pi
// to keep continuous angle value when acrossing pi(or -pi).
obj.theta = obj_raw_angle + obj_ang_index*2*3.141592; // converted angle
if ( fabs(obj.theta - obj.theta_prev) > 3.141592 ) {
if (obj.theta_prev > obj.theta) // pi to -pi region change
obj_ang_index++;
else
obj_ang_index--;
obj.theta = obj_raw_angle + obj_ang_index*2*3.141592; // newly converted angle
}
// calculation of the object angular velocity
obj.angVel = (obj.theta - obj.theta_prev) / sec; // the use of SAMPLE_RATE may not be a big difference.
// low pass filter
double alpha = 0.038; //0.02
obj.angVel = alpha*obj.angVel + (1-alpha)*obj.angVel_prev;
// calculation of the hand angular velocity
handTheta = -(HW->GetEncoderCount(IoHardware::ENC_0)) * ENC_RAD_PER_CNT / GR / 4; // just access the data previously read by GetEnc...()
handVel = (handTheta - handTheta_prev) /sec; //* SAMPLE_RATE;
handVel = alpha*handVel + (1-alpha)*handVel_prev;
// calculation of sh and \dot sh (=shD)
sh = (obj.theta - handTheta) / K_R;
eta2 = asin(-(obj.x-OBJ_X_OFFSET)/(RHO_H+RHO_O)); // alternative way to get eta2;
/// test, eta2 compensation
//eta2 = eta2 - 0.004*sin(obj.theta + 1.885);
// new method for shD, thus eta1 feedback
eta2D = (eta2 - eta2_prev) / sec;
eta2D = alpha*eta2D + (1-alpha)*eta2D_prev;
shD = RHO_H*(eta2D - handVel);
eta1 = M22*shD + M12*handVel;
/*************************************************************/
// calculation of the control input (acceleration command)
eta1Bar = eta1 - COEFF_ETA1_STAR;
z = handVel - TARGET_THETAHD;
if (eta2 == 0) {
u = -GAIN_K1*eta1Bar;
}
else {
u = -GAIN_K1*eta1Bar*sin(eta2)/eta2 - GAIN_K2*eta2;
}
alpha = (u - COEFF_sig2*eta1Bar)/COEFF_sig3;
if (eta2 == 0) {
DuDeta1Bar = -GAIN_K1;
DuDeta2 = -GAIN_K2;
}
else {
DuDeta1Bar = -GAIN_K1*sin(eta2)/eta2;
DuDeta2 = -GAIN_K1*eta1Bar*( (cos(eta2)*eta2 - sin(eta2))/(eta2*eta2) ) - GAIN_K2;
}
DalphaDeta1Bar = (DuDeta1Bar - COEFF_sig2)/COEFF_sig3;
DalphaDeta2 = DuDeta2/COEFF_sig3;
vInp = DalphaDeta1Bar*COEFF_sig1*sin(eta2) + DalphaDeta2*(COEFF_sig2*eta1Bar+COEFF_sig3*z) - eta2*COEFF_sig3 - GAIN_C*(z - alpha);
/*************************************************************/
aCmd = vInp; // calculated acceleration command
obj.theta_prev = obj.theta;
obj.angVel_prev = obj.angVel;
handTheta_prev = handTheta;
handVel_prev = handVel;
eta2D_prev = eta2D;
eta2_prev = eta2;
}
else { // Because the vision system keeps sending the data, QNX must read them, otherwise the vision system will get a send error.
VNET->Process();
}
// test
if (fabs(handVel) > 7.0 ) {//rad/sec
HW->SetAnalogOut(IoHardware::AMP_SIGNAL, 0.0);
HW->WriteDigitalBit(IoHardware::MOTOR_ENABLE, MOTOR_OFF);
HW->motorStatus = MOTOR_OFF;
//VNET->Process();
FATAL_ERROR("MOTOR RUNS TOO FAST");
lost = 1;
}
//**************************************************************
// reading & calculating output
//
// feedback
cycle = ClockCycles();
elapsedSec = (double)(cycle - cycle_prev)/cps;
cycle_prev = cycle;
//HW->ProcessInput(); // because of the encoder reading. Do not use this again. This takes a long time.
enc = -(HW->ReadEncoder(IoHardware::ENC_0)); // direct read. Not working?
pos = (enc * ENC_RAD_PER_CNT) / GR / 4; // convert to rad. GR:gear ratio (50). 4?
vel = (pos - pos_prev) / elapsedSec; // to calculate more exact velocity.
// This is necessary for not having motor run unexpectedly when swtiching from motor off to motor on.
if(HW->motorStatus == MOTOR_ON) {
cnt++;
// acceleration inner loop
// Notice that using no filtering velocity
vCmd = vCmd_prev + aCmd / SAMPLE_RATE;
pCmd = pCmd_prev + vCmd_prev / SAMPLE_RATE + 0.5 * aCmd / (SAMPLE_RATE*SAMPLE_RATE);
iCmd = calcI(vCmd, aCmd); // feedforward control.
feedforwardVal = iCmd;
//iCmd += (150* (pCmd - pos) + 0.6 * (vCmd - vel) + Ki * error); // Ki 0
//iCmd += (200* (pCmd - pos) + 0.6 * (vCmd - vel) + Ki * error); // Ki 0
//iCmd += (350* (pCmd - pos) + 1.0 * (vCmd - vel) + Ki * error); // Ki 0
iCmd += (150* (pCmd - pos) + 0.9 * (vCmd - vel) + Ki * error); // Ki 0
pos_prev = pos;
pCmd_prev = pCmd;
vCmd_prev = vCmd;
}
else {
iCmd = 0;
}
//**************************************************
//**************************************************
// control output
//
//limit current based on motor specs
if(iCmd > (MAX_CURRENT_MA) ){ // 1.6 mA
HW->SetAnalogOut(IoHardware::AMP_SIGNAL, 0.0);
HW->WriteDigitalBit(IoHardware::MOTOR_ENABLE, MOTOR_OFF);
HW->motorStatus = MOTOR_OFF;
//VNET->Process();
printf("iCmd:%f, pCmd:%f, pos:%f, vCmd:%f, vel:%f, aCmd:%f feedforwardVal:%f cnt:%d\n", iCmd, pCmd, pos, vCmd, vel, aCmd, feedforwardVal, cnt);
FATAL_ERROR("MOTOR CURRENT TOO HIGH");
lost = 1;
iCmd = MAX_CURRENT_MA;
}
else if(iCmd < (-MAX_CURRENT_MA) ){
HW->SetAnalogOut(IoHardware::AMP_SIGNAL, 0.0);
HW->WriteDigitalBit(IoHardware::MOTOR_ENABLE, MOTOR_OFF);
HW->motorStatus = MOTOR_OFF;
//VNET->Process();
printf("iCmd:%f, pCmd:%f, pos:%f, vCmd:%f, vel:%f, aCmd:%f feedforwardVal:%f cnt:%d%f\n", iCmd, pCmd, pos, vCmd, vel, aCmd, feedforwardVal, cnt);
FATAL_ERROR("MOTOR CURRENT TOO HIGH");
lost = 1;
iCmd = -MAX_CURRENT_MA;
}
ampVCmd =-iCmd * AMP_GAIN; // convert to analog signal. +-10 V. -0.86 for 4.5 rad/s. for test use -0.8
// sign change to agree with camera
// output signal to amp
if(!done){
HW->WriteAnalogCh(IoHardware::AMP_SIGNAL, ampVCmd);
//HW->WriteAnalogCh(IoHardware::AMP_SIGNAL, 0.0);
}
else{
HW->WriteAnalogCh(IoHardware::AMP_SIGNAL, 0.0);
}
//*******************************************************
//**************************************************
HW->ProcessOutput(); // all digital & analog writing.
// set outputs
/*num[0] = obj.theta;
num[1] = obj.angVel;
*/
num[0] = vInp;//obj.theta; //vInp; //correctedAcc; //eta1; //handVel; //pos; //handTheta; //pos; // angle in rad
num[1] = eta1;//obj.angVel; //eta1; //feedforwardVal; //pCmd; //eta2; //obj.theta; //vnum[0]; //obj.theta; //vel; //obj.theta;
num[2] = pos; //feedbackVal; //vCmd; //obj.angVel; //obj.x; //vnum[1]; //obj.x1; //ampVCmd; //obj.x1; // position in m, *10 for cm
num[3] = eta2; //handTheta; //iCmd; //sec; //elapsedSec; //sec; //vnum[2]; //obj.y1;
MNET->Process();
} // while()
// for some reason, this does not work.
HW->SetAnalogOut(IoHardware::AMP_SIGNAL, 0.0);
HW->WriteDigitalBit(IoHardware::MOTOR_ENABLE, MOTOR_OFF);
HW->motorStatus = MOTOR_OFF;
delay(10);
FATAL_ERROR(err_msg);
}
void SampleLoopTask::Task() {
int cnt = 0;
int region;
int attempt;
static int dout = 1;
static double error = 0;
/*static int last_status = 0;
static double pos_prev = 0;
static double pCmd_prev = 0;
static double vCmd_prev = 0;
*/
int last_status = 0;
double pos_prev = 0;
double pCmd_prev = 0;
double vCmd_prev = 0;
int obj_ang_index = 0; // to keep continuous angle value when acrossing pi(or -pi).
double obj_x_offset = 0;
//double obj_y_offset = 0;
uint64_t cycle, cycle_prev;
// for acceleration calculation
double z = 0; //!
double h = 0; //! linearizing output
double hD = 0; //!
double hDD = 0; //!
double hDDD = 0; //!
double u =0; //
double eta2_prev = 0; //!
double eta2D = 0; //! to use eta2D in calculation of shD
double eta2D_prev = 0; //! to use eta2D in calculation of shD
double eta1_prev = 0; //!
double eta1D = 0; //! to use eta1D instead of sigma1 * sin(eta2)
double eta1D_prev = 0; //! to use eta1D instead of sigma1 * sin(eta2)
//double eta_a = 0.015;
//double eta_b = 0.02;
//double ETA2 = 0;
//double SIN_ETA2 = 0;
//int eta1D_flag = 0;
double err_sum = 0.0;
// for SMC
/*double s = 0;
double lambda = 3;
double eta = 0.1;
double F = 0;
double k = 0;
double fHat = 0;
double uHat = 0;
*/
//test
double feedforwardVal = 0;
double feedbackVal = 0;
//double test_v_prev = 0;
//double vCmd_prev = 0;
double elapsedSec = 0;
uint64_t cycle1, cycle2;
double elapsedSec1 = 0;
double elapsedSec2 = 0;
double obj_vel_uf = 0;
double vInp_prev = 0;
//double aCmd_sin = 0;
//int update_done = 0; // for automatic x_offset update
//double x_avg = 0; // for automatic x_offset update
//double x_max = 0;
//double x_min = 0;
while(!lost) {
TimerWait();
ncycles = ClockCycles();
sec=(double)(ncycles - ncycles_prev)/cps;
ncycles_prev = ncycles;
TimingProcess();
// Read Inputs
HW->ProcessInput(); // all digital & analog, encoders reading.
// Get status of camera
last_status = HW->ReadDigitalBit(IoHardware::FRAME_STATUS);
// Send out pulse to trigger camera
HW->WriteDigitalBit(IoHardware::CAMERA_TRIGGER, 1);
HW->WriteDigitalBit(IoHardware::CAMERA_TRIGGER, 0);
// test
//cycle1 = ClockCycles();
if(camera) {// && !lost){
// Wait for camera to process data, with timeout counter
while(HW->ReadDigitalBit(IoHardware::FRAME_STATUS) == last_status) {
if(++attempt == (int)(5.0e5/SAMPLE_RATE)) {
HW->SetAnalogOut(IoHardware::AMP_SIGNAL, 0.0);
HW->WriteDigitalBit(IoHardware::MOTOR_ENABLE, MOTOR_OFF);
HW->motorStatus = MOTOR_OFF;
delay(10);
FATAL_ERROR("Frame not received -");
//strcpy(err_msg, "Frame not received -");
lost = 1;
}
}
attempt = 0;
// test
//cycle2 = ClockCycles();
//elapsedSec1 = (double)(cycle2 - cycle1)/cps;
vnum[0] = -99;
int n = VNET->Recv();
//test
//cycle1 = ClockCycles();
//elapsedSec2 = (double)(cycle1 - cycle2)/cps;
region = (int)vnum[0];
switch(region) {
case 0: //ROI_0:
obj.x0 = vnum[1]; //mm
obj.y0 = vnum[2]; //mm
break;
case 1: //ROI_1:
obj.x1 = vnum[1]; //mm
obj.y1 = vnum[2]; //mm
break;
default:
//HW->SetAnalogOut(IoHardware::AMP_SIGNAL, 0.0);
//HW->WriteDigitalBit(IoHardware::MOTOR_ENABLE, MOTOR_OFF);
printf("roi-vnum[0]:%d\n", region);
FATAL_ERROR("Object lost -");
//strcpy(err_msg, "Object lost -");
lost = 1;
}
obj.x = (obj.x0 + obj.x1)/2/1000; // convert to m
obj.y = (obj.y0 + obj.y1)/2/1000;
double obj_raw_angle = atan2(obj.y1-obj.y0, obj.x1-obj.x0); // within -pi to pi
// compensating angle offset
// set only once at the beginning
/*if (obj_x_offset == 0 && obj.x0 != 0 && obj.x1 != 0) {
obj_x_offset = obj.x;
obj_y_offset = obj.y;
}*/
// object center position compensation
//obj.x = obj.x - 0.000250544*cos(obj_raw_angle + (-2.410447));
//uint64_t cycle1 = ClockCycles();
// to keep continuous angle value when acrossing pi(or -pi).
obj.theta = obj_raw_angle + obj_ang_index*2*3.141592; // converted angle
if ( fabs(obj.theta - obj.theta_prev) > 3.141592 ) {
if (obj.theta_prev > obj.theta) // pi to -pi region change
obj_ang_index++;
else
obj_ang_index--;
obj.theta = obj_raw_angle + obj_ang_index*2*3.141592; // newly converted angle
}
// calculation of the object angular velocity
//obj.angVel = (obj.theta - obj.theta_prev) * SAMPLE_RATE;
obj.angVel = (obj.theta - obj.theta_prev) / sec;
obj_vel_uf = obj.angVel;
// low pass filter
double alpha = 0.038; //0.02
obj.angVel = alpha*obj.angVel + (1-alpha)*obj.angVel_prev;
// calculation of the hand angular velocity
handTheta = -(HW->GetEncoderCount(IoHardware::ENC_0)) * ENC_RAD_PER_CNT / GR / 4; // just access the data previously read by GetEnc...()
handVel = (handTheta - handTheta_prev) /sec; //* SAMPLE_RATE;
//alpha = 0.1;
handVel = alpha*handVel + (1-alpha)*handVel_prev;
// calculation of sh and \dot sh (=shD)
sh = (obj.theta - handTheta) / K_R;
//shD = (obj.angVel - handVel) / K_R;
// coordinate transformation
//eta1 = M22*shD + M12*handVel;
// eta1D
//eta1D = (eta1 - eta1_prev) / sec;
//eta1D = alpha*eta1D + (1-alpha)*eta1D_prev;
//eta2 = handTheta + sh/RHO_H;
//obj_x_offset = OBJ_X_OFFSET + 0.0010775*sin(4*handTheta + 0.6422); // 0.0916609 or OBJ_X_OFFSET
//eta2 = asin(-(obj.x-obj_x_offset)/(RHO_H+RHO_O)); // alternative way to get eta2;
eta2 = asin(-(obj.x-OBJ_X_OFFSET)/(RHO_H+RHO_O)); // alternative way to get eta2;
//eta2 = atan(-(obj.x-OBJ_X_OFFSET)/(obj.y-OBJ_Y_OFFSET+RHO_H+RHO_O)); // alternative way to get eta2;
// xi = handVel;
//eta2 compensation
//eta2 = eta2 - 0.004*sin(obj.theta + 1.885);
// new method for shD, thus eta1 feedback
eta2D = (eta2 - eta2_prev) / sec;
eta2D = alpha*eta2D + (1-alpha)*eta2D_prev;
shD = RHO_H*(eta2D - handVel);
eta1 = M22*shD + M12*handVel;
// eta1D
// don't use this. this creates motor current too high
//eta1D = (eta1 - eta1_prev) / sec;
//eta1D = 0.005*eta1D + (1-0.005)*eta1D_prev;
//SIN_ETA2 = sin(eta2);
/*ETA2 = eta2;
if ( fabs(eta2) < eta_b ) {
if (eta2 > eta_a) {
//SIN_ETA2 = sin(2*eta2 - 0.02);
ETA2 = eta_b/(eta_b - eta_a)*(eta2 - eta_a);
}
else if (eta2 < -eta_a) {
ETA2 = eta_b/(eta_b - eta_a)*(eta2 + eta_a);
}
else {
ETA2 = 0;
}
}*/
// original
/*************************************************************/
// calculation of the control input (acceleration command)
z = handTheta - TARGET_THETAH;
h = eta2 - COEFF_sig3*z;
hD = eta2D - COEFF_sig3*handVel; // slightly better
//hD = COEFF_sig2*eta1;
hDD = COEFF_sig2*COEFF_sig1*sin(eta2);
//hDD = COEFF_sig2*eta1D; // use directly calculated (filtered) value for eta1D
/*if (eta1D_flag == 0) {
if ( fabs(eta1D - COEFF_sig2*sin(eta2)) < 0.02 ) {
eta1D_flag = 1;
}
else {
hDD = COEFF_sig2*COEFF_sig1*sin(eta2);
}
}*/
hDDD = COEFF_sig2*COEFF_sig1*cos(eta2)*eta2D; // slightly better
//hDDD = COEFF_sig2*COEFF_sig1*cos(eta2)*(COEFF_sig2*eta1 + COEFF_sig3*handVel);
/* For addition of integral control */
if(HW->motorStatus == MOTOR_ON) {
err_sum = err_sum + z*sec;
}
else {
err_sum = 0;
} // does not working successfully.
/// test
err_sum = 0;
u = -GAIN_K1*hDDD - GAIN_K2*hDD - GAIN_K3*hD - GAIN_K4*h + 100*err_sum;
vInp = ( u - COEFF_sig2*COEFF_sig1*sin(eta2)*(-eta2D + COEFF_sig1*COEFF_sig2*cos(eta2)) )/(COEFF_sig1*COEFF_sig2*COEFF_sig3*cos(eta2)); // slightly better
//vInp = ( u - hDD*(-eta2D + COEFF_sig1*COEFF_sig2*cos(eta2)) )/(COEFF_sig1*COEFF_sig2*COEFF_sig3*cos(eta2));
//vInp = ( u - COEFF_sig2*COEFF_sig1*sin(eta2)*(-COEFF_sig2*eta1-COEFF_sig3*handVel + COEFF_sig1*COEFF_sig2*cos(eta2)) )/(COEFF_sig1*COEFF_sig2*COEFF_sig3*cos(eta2));
//vInp = ( u - hDD*(-COEFF_sig2*eta1-COEFF_sig3*handVel + COEFF_sig1*COEFF_sig2*cos(eta2)) )/(COEFF_sig1*COEFF_sig2*COEFF_sig3*cos(eta2));
//vInp = ( u - COEFF_sig2*COEFF_sig1*sin(eta2)*(-COEFF_sig2*eta1-COEFF_sig3*handVel) - COEFF_sig1*COEFF_sig2*cos(eta2)*hDD)/(COEFF_sig1*COEFF_sig2*COEFF_sig3*cos(eta2));
/*************************************************************/
// Sliding Mode Control
/*s = hDDD + 3*lambda*hDD + 3*lambda*lambda*hD + lambda*lambda*lambda*h;
F = 0.02 * COEFF_sig1*COEFF_sig2*abs( -eta2D + COEFF_sig1*COEFF_sig2*cos(eta2) );
k = F + eta;
fHat = COEFF_sig1*COEFF_sig2*( -eta2D + COEFF_sig1*COEFF_sig2*cos(eta2) ) * eta2;
uHat = -fHat - 3*lambda*hDDD - 3*lambda*lambda*hDD - lambda*lambda*lambda*hD;
u = uHat - k*(s > 0 ? 1 : (s < 0 ? -1 : 0));
vInp = u /(COEFF_sig1*COEFF_sig2*COEFF_sig3*cos(eta2));
*/
//test input filter
//vInp = 0.5*vInp + (1-0.5)*vInp_prev;
aCmd = vInp; // calculated acceleration command
obj.theta_prev = obj.theta;
obj.angVel_prev = obj.angVel;
handTheta_prev = handTheta;
handVel_prev = handVel;
eta2D_prev = eta2D;
eta2_prev = eta2;
eta1_prev = eta1;
eta1D_prev = eta1D;
vInp_prev = vInp;
// For resetting of the upright eq. position
/*if(HW->motorStatus == MOTOR_ON && update_done == 0) {
x_avg += obj.x;
if (obj.x > x_max) {
x_max = obj.x;
}
if (obj.x < x_min) {
x_min = obj.x;
}
if ( (cnt%int(SAMPLE_RATE)) == 1 ) {
x_avg = x_avg / SAMPLE_RATE;
if ( (x_max - x_avg) < 0.001 && (x_avg - x_min) < 0.001 ) {
obj_x_offset = x_avg;
//x_offset_test = x_avg;
update_done = 1;
}
else {
x_avg = 0;
x_max = obj.x;
x_min = obj.x;
}
}
}*/
//uint64_t cycle2 = ClockCycles();
//elapsedSec=(double)(cycle2 - cycle1)/cps;
}
else { // Because the vision system keeps sending the data, QNX must read them, otherwise the vision system will get a send error.
VNET->Process();
//obj_ang_offset = 0.0; // reset the angle offset
//aCmd = 0;
//vCmd_prev = 0;
//pCmd_prev = 0;
}
// test
if (fabs(handVel) > 7.0 ) {//rad/sec
HW->SetAnalogOut(IoHardware::AMP_SIGNAL, 0.0);
HW->WriteDigitalBit(IoHardware::MOTOR_ENABLE, MOTOR_OFF);
HW->motorStatus = MOTOR_OFF;
delay(10);
FATAL_ERROR("MOTOR RUNS TOO FAST");
lost = 1;
}
//**************************************************************
// reading & calculating output
//
// feedback
cycle = ClockCycles();
elapsedSec = (double)(cycle - cycle_prev)/cps;
cycle_prev = cycle;
//HW->ProcessInput(); // because of the encoder reading. Do not use this again. This takes a long time.
//cycle2 = ClockCycles();
enc = -(HW->ReadEncoder(IoHardware::ENC_0)); // direct read. Not working?
//cycle1 = ClockCycles();
//elapsedSec2 = (double)(cycle1 - cycle2)/cps;
//enc = -(HW->GetEncoderCount(IoHardware::ENC_0)); // sign change to agree with camera
pos = (enc * ENC_RAD_PER_CNT) / GR / 4; // convert to rad. GR:gear ratio (50). 4?
//vel = (pos - pos_prev) * SAMPLE_RATE;
vel = (pos - pos_prev) / elapsedSec; // to calculate more exact velocity.
// This is necessary for not having motor run unexpectedly when swtiching from motor off to motor on.
if(HW->motorStatus == MOTOR_ON) {
cnt++;
/*double T_period = 0.2; //2;
// sinusoidal wave
aCmd = -2*DEG2RAD*(2*3.141592/T_period)*(2*3.141592/T_period)*sin(2*3.141592*(cnt)/SAMPLE_RATE/T_period);
//aCmd_sin = aCmd;
if (cnt == 1) { // at the beginning
// because of the sinusoidal wave
vCmd_prev = 2*DEG2RAD*(2*3.141592/T_period);
}
*/
// acceleration inner loop
// Notice that using no filtering velocity
vCmd = vCmd_prev + aCmd / SAMPLE_RATE;
pCmd = pCmd_prev + vCmd_prev / SAMPLE_RATE + 0.5 * aCmd / (SAMPLE_RATE*SAMPLE_RATE);
//pCmd = pCmd_prev + vCmd / SAMPLE_RATE;
//vCmd = vCmd_prev + aCmd * elapsedSec;
//pCmd = pCmd_prev + vCmd * elapsedSec;
iCmd = calcI(vCmd, aCmd); // feedforward control.
feedforwardVal = iCmd;
//iCmd += (150* (pCmd - pos) + 0.6 * (vCmd - vel) + Ki * error); // Ki 0
//iCmd += (200* (pCmd - pos) + 0.6 * (vCmd - vel) + Ki * error); // Ki 0
//iCmd += (350* (pCmd - pos) + 1.0 * (vCmd - vel) + Ki * error); // Ki 0
iCmd += (150* (pCmd - pos) + 0.9 * (vCmd - vel) + Ki * error); // Ki 0
pos_prev = pos;
pCmd_prev = pCmd;
vCmd_prev = vCmd;
}
else {
iCmd = 0;
}
//**************************************************
//**************************************************
// control output
//
//limit current based on motor specs
if(iCmd > (MAX_CURRENT_MA) ) { // 1.6 mA
HW->SetAnalogOut(IoHardware::AMP_SIGNAL, 0.0);
HW->WriteDigitalBit(IoHardware::MOTOR_ENABLE, MOTOR_OFF);
HW->motorStatus = MOTOR_OFF;
printf("iCmd:%f, pCmd:%f, pos:%f, vCmd:%f, vel:%f, aCmd:%f feedforwardVal:%f cnt:%d\n", iCmd, pCmd, pos, vCmd, vel, aCmd, feedforwardVal, cnt);
delay(10);
FATAL_ERROR("MOTOR CURRENT TOO HIGH");
lost = 1;
iCmd = MAX_CURRENT_MA;
}
else if(iCmd < (-MAX_CURRENT_MA) ) {
HW->SetAnalogOut(IoHardware::AMP_SIGNAL, 0.0);
HW->WriteDigitalBit(IoHardware::MOTOR_ENABLE, MOTOR_OFF);
HW->motorStatus = MOTOR_OFF;
printf("iCmd:%f, pCmd:%f, pos:%f, vCmd:%f, vel:%f, aCmd:%f feedforwardVal:%f cnt:%d%f\n", iCmd, pCmd, pos, vCmd, vel, aCmd, feedforwardVal, cnt);
delay(10);
FATAL_ERROR("MOTOR CURRENT TOO HIGH");
lost = 1;
iCmd = -MAX_CURRENT_MA;
}
ampVCmd =-iCmd * AMP_GAIN; // convert to analog signal. +-10 V. -0.86 for 4.5 rad/s. for test use -0.8
// sign change to agree with camera
// output signal to amp
if(!done) {
HW->WriteAnalogCh(IoHardware::AMP_SIGNAL, ampVCmd);
//HW->WriteAnalogCh(IoHardware::AMP_SIGNAL, 0.0);
}
else {
HW->WriteAnalogCh(IoHardware::AMP_SIGNAL, 0.0);
}
//*******************************************************
//**************************************************
HW->ProcessOutput(); // all digital & analog writing.
// keep the position angle within 2pi
//if ( pos > (mu_k+1)*2*3.141592 )
// mu_k++;
//pos = pos - mu_k*2*3.141592;
/*num[0] = sec; //aCmd; //correctedAcc; //eta1; //handVel; //pos; //handTheta; //pos; // angle in rad
num[1] = elapsedSec1; //test_v_prev; //feedforwardVal; //pCmd; //eta2; //obj.theta; //vnum[0]; //obj.theta; //vel; //obj.theta;
num[2] = elapsedSec2; //feedbackVal; //vCmd; //obj.angVel; //obj.x; //vnum[1]; //obj.x1; //ampVCmd; //obj.x1; // position in m, *10 for cm
num[3] = vCmd; //handTheta; //iCmd; //sec; //elapsedSec; //sec; //vnum[2]; //obj.y1;
*/
// set outputs
num[0] = vInp; //correctedAcc; //eta1; //handVel; //pos; //handTheta; //pos; // angle in rad
num[1] = eta1; //feedforwardVal; //pCmd; //eta2; //obj.theta; //vnum[0]; //obj.theta; //vel; //obj.theta;
num[2] = pos; //feedbackVal; //vCmd; //obj.angVel; //obj.x; //vnum[1]; //obj.x1; //ampVCmd; //obj.x1; // position in m, *10 for cm
num[3] = eta2; //handTheta; //iCmd; //sec; //elapsedSec; //sec; //vnum[2]; //obj.y1;
// acceleration innerloop
/*num[0] = aCmd; //aCmd;
num[1] = pCmd;
num[2] = pos;
//num[1] = feedforwardVal;
//num[2] = feedbackVal;
num[3] = vCmd;
*/
// motor modeling
/*num[0] = pos;
num[1] = iCmd;
num[2] = cnt;
num[3] = 0;
*/
MNET->Process();
} // while()
// for some reason, this does not work.
HW->SetAnalogOut(IoHardware::AMP_SIGNAL, 0.0);
HW->WriteDigitalBit(IoHardware::MOTOR_ENABLE, MOTOR_OFF);
HW->motorStatus = MOTOR_OFF;
delay(10);
FATAL_ERROR(err_msg);
}
void SampleLoopTask::Task() {
int cnt = 0;
int region;
int attempt;
static int dout = 1;
static int last_status = 0;
static double pos_prev = 0;
static double pCmd_prev = 0;
static double vCmd_prev = 0;
static double error = 0;
int temp = 0;
int mu_k = 0; // for test
int obj_ang_index = 0; // to keep continuous angle value when acrossing pi(or -pi).
//double obj_ang_offset = 0.0; // to subtract the object's angle at equilibrium position when inputting 'c' (camera)
double obj_x_offset = 0;
double obj_y_offset = 0;
uint64_t cycle, cycle_prev;
//test
double eta1_prev = 0;
double feedforwardVal = 0;
double feedbackVal = 0;
double test_v_prev = 0;
//double vCmd_prev = 0;
double elapsedSec = 0;
uint64_t cycle1, cycle2;
double elapsedSec1 = 0;
double elapsedSec2 = 0;
double aCmd_sin = 0;
double obj_vel_uf = 0;
double vInp_prev = 0;
double x_avg = 0; // for automatic x_offset update
double x_max = 0;
double x_min = 0;
//int update_count = 0; // for automatic x_offset update
//int update_trigger = 0; // for automatic x_offset update
int update_done = 0; // for automatic x_offset update
double x_offset_test = 0;
int switching_done = 0; // for controller swtiching
while(!lost) {
TimerWait();
ncycles = ClockCycles();
sec=(double)(ncycles - ncycles_prev)/cps;
ncycles_prev = ncycles;
TimingProcess();
// Read Inputs
HW->ProcessInput(); // all digital & analog, encoders reading.
// Get status of camera
last_status = HW->ReadDigitalBit(IoHardware::FRAME_STATUS);
// Send out pulse to trigger camera
HW->WriteDigitalBit(IoHardware::CAMERA_TRIGGER, 1);
HW->WriteDigitalBit(IoHardware::CAMERA_TRIGGER, 0);
// test
//cycle1 = ClockCycles();
if(camera) {// && !lost){
// Wait for camera to process data, with timeout counter
while(HW->ReadDigitalBit(IoHardware::FRAME_STATUS) == last_status) {
if(++attempt == (int)(1e8/SAMPLE_RATE)) {
HW->SetAnalogOut(IoHardware::AMP_SIGNAL, 0.0);
HW->WriteDigitalBit(IoHardware::MOTOR_ENABLE, MOTOR_OFF);
FATAL_ERROR("Frame not received -");
//strcpy(err_msg, "Frame not received -");
lost = 1;
}
}
attempt = 0;
// test
//cycle2 = ClockCycles();
//elapsedSec1 = (double)(cycle2 - cycle1)/cps;
vnum[0] = -99;
int n = VNET->Recv();
//test
//cycle1 = ClockCycles();
//elapsedSec2 = (double)(cycle1 - cycle2)/cps;
region = (int)vnum[0];
switch(region) {
case 0: //ROI_0:
obj.x0 = vnum[1]; //mm
obj.y0 = vnum[2]; //mm
break;
case 1: //ROI_1:
obj.x1 = vnum[1]; //mm
obj.y1 = vnum[2]; //mm
break;
default:
//HW->SetAnalogOut(IoHardware::AMP_SIGNAL, 0.0);
//HW->WriteDigitalBit(IoHardware::MOTOR_ENABLE, MOTOR_OFF);
printf("roi-vnum[0]:%d\n", region);
FATAL_ERROR("Object lost -");
//strcpy(err_msg, "Object lost -");
lost = 1;
}
obj.x = (obj.x0 + obj.x1)/2/1000; // convert to m
obj.y = (obj.y0 + obj.y1)/2/1000;
double obj_raw_angle = atan2(obj.y1-obj.y0, obj.x1-obj.x0); // within -pi to pi
// compensating angle offset
// set only once at the beginning
if (obj_x_offset == 0 && obj.x0 != 0 && obj.x1 != 0) {
obj_x_offset = obj.x;
obj_y_offset = obj.y;
x_offset_test = obj.x; // test
}
//uint64_t cycle1 = ClockCycles();
// to keep continuous angle value when acrossing pi(or -pi).
obj.theta = obj_raw_angle + obj_ang_index*2*3.141592; // converted angle
if ( fabs(obj.theta - obj.theta_prev) > 3.141592 ) {
if (obj.theta_prev > obj.theta) // pi to -pi region change
obj_ang_index++;
else
obj_ang_index--;
obj.theta = obj_raw_angle + obj_ang_index*2*3.141592; // newly converted angle
}
// calculation of the object angular velocity
//obj.angVel = (obj.theta - obj.theta_prev) * SAMPLE_RATE;
obj.angVel = (obj.theta - obj.theta_prev) / sec;
obj_vel_uf = obj.angVel;
// low pass filter
double alpha = 0.038; //0.06; //0.038; //0.02
obj.angVel = alpha*obj.angVel + (1-alpha)*obj.angVel_prev;
// calculation of the hand angular velocity
handTheta = -(HW->GetEncoderCount(IoHardware::ENC_0)) * ENC_RAD_PER_CNT / GR / 4; // just access the data previously read by GetEnc...()
handVel = (handTheta - handTheta_prev) / sec; //* SAMPLE_RATE;
//alpha = 0.1;
handVel = alpha*handVel + (1-alpha)*handVel_prev;
// calculation of sh and \dot sh (=shD)
sh = (obj.theta - handTheta) / K_R;
shD = (obj.angVel - handVel) / K_R;
// coordinate transformation
eta1 = M22*shD + M12*handVel;
//test filtering
//eta1 = alpha*eta1 + (1-alpha)*eta1_prev;
//eta1_prev = eta1;
//eta2 = handTheta + sh/RHO_H;
eta2 = asin(-(obj.x-obj_x_offset)/(RHO_H+RHO_O)); // alternative way to get eta2;
// calculation of the control input (acceleration command)
//xi := handVel;
//eta1D := COEFF_D * sin(eta2); // COEFF_D
//eta2D := COEFF_A * eta1 - xi/COEFF_B;
//double GAIN_C2 = GAIN_C2_FAR;
double GAIN_C2 = GAIN_C2_FAR;
/*if ( abs(eta2) < 0.03 && abs(handVel) < 0.2) { //0.015 ) { // eta2=0.015(0.86 deg) : 3.45 mm in x
//if ( abs(eta2) < 0.03 ) { //0.015 ) {
GAIN_C2 = GAIN_C2_NEAR;
}*/
double TANH = tanh(GAIN_C1*eta1 + GAIN_C2*eta2);
alpha_eta = COEFF_B * (COEFF_A*eta1 + GAIN_C0*TANH + GAIN_C3*eta2);
DalphaDeta1 = COEFF_B * ( COEFF_A + GAIN_C0*(1-TANH*TANH)*GAIN_C1 );
DalphaDeta2 = COEFF_B * ( GAIN_C0*(1-TANH*TANH)*GAIN_C2 + GAIN_C3 );
vInp = -GAIN_C * (handVel - alpha_eta) + DalphaDeta1*(COEFF_D * sin(eta2)) + DalphaDeta2*(COEFF_A * eta1 - handVel/COEFF_B);
//vInp = 0.05*vInp + (1-0.05)*vInp_prev;
aCmd = vInp; // calculated acceleration command
obj.theta_prev = obj.theta;
obj.angVel_prev = obj.angVel;
handTheta_prev = handTheta;
handVel_prev = handVel;
vInp_prev = vInp;
// automatic x offset update (old)
/*if (eta2*eta2 + handVel*handVel/100 < 0.00325) {
update_trigger = 1;
}
if (update_trigger == 1 && update_done != 1) {
update_count += 1;
x_avg += obj.x;
if (update_count > SAMPLE_RATE) { // averaging for 1 sec.
//obj_x_offset = x_avg / update_count;
x_offset_test = x_avg / update_count;
update_trigger = 0;
update_count = 0;
update_done = 1;
}
}*/
// automatic x offset update
/*if(HW->motorStatus == MOTOR_ON && update_done == 0) {
x_avg += obj.x;
if (obj.x > x_max) {
x_max = obj.x;
}
if (obj.x < x_min) {
x_min = obj.x;
}
if ( (cnt%int(SAMPLE_RATE)) == 1 ) {
x_avg = x_avg / SAMPLE_RATE;
if ( (x_max - x_avg) < 0.001 && (x_avg - x_min) < 0.001 ) {
//obj_x_offset = x_avg;
x_offset_test = x_avg;
update_done = 1;
}
else {
x_avg = 0;
x_max = obj.x;
x_min = obj.x;
}
}
}
*/
// Switching to a Linear Controller
/*if (HW->motorStatus == MOTOR_ON) {
if (switching_done == 1) {
aCmd = -(2565.2*eta1 + 292.0*eta2 + 16.1*handVel); // pole placement:sigma = 2.3, zeta = 0.85, third pole at 5sigma
}
else if ( eta2*eta2 + 0.01*handVel*handVel < 0.003125 ) {
switching_done = 1;
}
}*/
//uint64_t cycle2 = ClockCycles();
//elapsedSec=(double)(cycle2 - cycle1)/cps;
}
else { // Because the vision system keeps sending the data, QNX must read them, otherwise the vision system will get a send error.
VNET->Process();
//obj_ang_offset = 0.0; // reset the angle offset
//aCmd = 0;
//vCmd_prev = 0;
//pCmd_prev = 0;
}
// test
if (fabs(handVel) > 5.0 ) {//rad/sec
HW->SetAnalogOut(IoHardware::AMP_SIGNAL, 0.0);
HW->WriteDigitalBit(IoHardware::MOTOR_ENABLE, MOTOR_OFF);
HW->motorStatus = MOTOR_OFF;
FATAL_ERROR("MOTOR RUNS TOO FAST");
}
//**************************************************************
// reading & calculating output
//
// feedback
cycle = ClockCycles();
elapsedSec = (double)(cycle - cycle_prev)/cps;
cycle_prev = cycle;
//HW->ProcessInput(); // because of the encoder reading. Do not use this again. This takes a long time.
//cycle2 = ClockCycles();
enc = -(HW->ReadEncoder(IoHardware::ENC_0)); // direct read. Not working?
//cycle1 = ClockCycles();
//elapsedSec2 = (double)(cycle1 - cycle2)/cps;
//enc = -(HW->GetEncoderCount(IoHardware::ENC_0)); // sign change to agree with camera
pos = (enc * ENC_RAD_PER_CNT) / GR / 4; // convert to rad. GR:gear ratio (50). 4?
//vel = (pos - pos_prev) * SAMPLE_RATE;
vel = (pos - pos_prev) / elapsedSec; // to calculate more exact velocity.
// This is necessary for not having motor run unexpectedly when swtiching from motor off to motor on.
if(HW->motorStatus == MOTOR_ON) {
cnt++;
/*double T_period = 0.2; //2;
// sinusoidal wave
aCmd = -2*DEG2RAD*(2*3.141592/T_period)*(2*3.141592/T_period)*sin(2*3.141592*(cnt)/SAMPLE_RATE/T_period);
//aCmd_sin = aCmd;
if (cnt == 1) { // at the beginning
// because of the sinusoidal wave
vCmd_prev = 2*DEG2RAD*(2*3.141592/T_period);
}
*/
// acceleration inner loop
// Notice that using no filtering velocity
vCmd = vCmd_prev + aCmd / SAMPLE_RATE;
pCmd = pCmd_prev + vCmd_prev / SAMPLE_RATE + 0.5 * aCmd / (SAMPLE_RATE*SAMPLE_RATE);
//pCmd = pCmd_prev + vCmd / SAMPLE_RATE;
//vCmd = vCmd_prev + aCmd * elapsedSec;
//pCmd = pCmd_prev + vCmd * elapsedSec;
//test
test_v_prev = vCmd_prev;
iCmd = calcI(vCmd, aCmd); // feedforward control. Not being used currently.
feedforwardVal = iCmd;
//iCmd += (150* (pCmd - pos) + 0.6 * (vCmd - vel) + Ki * error); // Ki 0
//iCmd += (200* (pCmd - pos) + 0.6 * (vCmd - vel) + Ki * error); // Ki 0
//iCmd += (350* (pCmd - pos) + 1.0 * (vCmd - vel) + Ki * error); // Ki 0
iCmd += (150* (pCmd - pos) + 0.9 * (vCmd - vel) + Ki * error); // Ki 0
pos_prev = pos;
pCmd_prev = pCmd;
vCmd_prev = vCmd;
}
else {
iCmd = 0;
}
//**************************************************
//**************************************************
// control output
//
//limit current based on motor specs
if(iCmd > (MAX_CURRENT_MA) ) { // 1.6 mA
HW->SetAnalogOut(IoHardware::AMP_SIGNAL, 0.0);
HW->WriteDigitalBit(IoHardware::MOTOR_ENABLE, MOTOR_OFF);
HW->motorStatus = MOTOR_OFF;
printf("iCmd:%f, pCmd:%f, pos:%f, vCmd:%f, vel:%f, feedforwardVal:%f\n", iCmd, pCmd, pos, vCmd, vel, feedforwardVal);
FATAL_ERROR("MOTOR CURRENT TOO HIGH");
iCmd = MAX_CURRENT_MA;
}
else if(iCmd < (-MAX_CURRENT_MA) ) {
HW->SetAnalogOut(IoHardware::AMP_SIGNAL, 0.0);
HW->WriteDigitalBit(IoHardware::MOTOR_ENABLE, MOTOR_OFF);
HW->motorStatus = MOTOR_OFF;
printf("iCmd:%f, pCmd:%f, pos:%f, vCmd:%f, vel:%f, feedforwardVal:%f\n", iCmd, pCmd, pos, vCmd, vel, feedforwardVal);
FATAL_ERROR("MOTOR CURRENT TOO HIGH");
iCmd = -MAX_CURRENT_MA;
}
ampVCmd =-iCmd * AMP_GAIN; // convert to analog signal. +-10 V. -0.86 for 4.5 rad/s. for test use -0.8
// sign change to agree with camera
// output signal to amp
if(!done) {
HW->WriteAnalogCh(IoHardware::AMP_SIGNAL, ampVCmd);
//HW->WriteAnalogCh(IoHardware::AMP_SIGNAL, 0.0);
}
else {
HW->WriteAnalogCh(IoHardware::AMP_SIGNAL, 0.0);
}
//*******************************************************
//**************************************************
HW->ProcessOutput(); // all digital & analog writing.
// keep the position angle within 2pi
//if ( pos > (mu_k+1)*2*3.141592 )
// mu_k++;
//pos = pos - mu_k*2*3.141592;
/*num[0] = sec; //aCmd; //correctedAcc; //eta1; //handVel; //pos; //handTheta; //pos; // angle in rad
num[1] = elapsedSec1; //test_v_prev; //feedforwardVal; //pCmd; //eta2; //obj.theta; //vnum[0]; //obj.theta; //vel; //obj.theta;
num[2] = elapsedSec2; //feedbackVal; //vCmd; //obj.angVel; //obj.x; //vnum[1]; //obj.x1; //ampVCmd; //obj.x1; // position in m, *10 for cm
num[3] = vCmd; //handTheta; //iCmd; //sec; //elapsedSec; //sec; //vnum[2]; //obj.y1;
*/
// set outputs
/*num[0] = obj_vel_uf; //vInp; //correctedAcc; //eta1; //handVel; //pos; //handTheta; //pos; // angle in rad
num[1] = vel; //eta1; //feedforwardVal; //pCmd; //eta2; //obj.theta; //vnum[0]; //obj.theta; //vel; //obj.theta;
num[2] = obj.angVel; //feedbackVal; //vCmd; //obj.angVel; //obj.x; //vnum[1]; //obj.x1; //ampVCmd; //obj.x1; // position in m, *10 for cm
num[3] = aCmd; //handTheta; //iCmd; //sec; //elapsedSec; //sec; //vnum[2]; //obj.y1;
*/
num[0] = handVel;
num[1] = aCmd; //x_offset_test; // //obj_x_offset;
num[2] = switching_done; //obj.x;
num[3] = eta2;
/*num[0] = handVel;
num[1] = eta1;
num[2] = vel;
num[3] = eta2;
*/
/*num[0] = vInp;
num[1] = eta1;
num[2] = pos;
num[3] = eta2;
*/
// acceleration innerloop
/*num[0] = aCmd;
num[1] = pCmd;
num[2] = pos;
//num[1] = feedforwardVal;
//num[2] = feedbackVal;
num[3] = vCmd;
*/
// motor modeling
/*num[0] = pos;
num[1] = iCmd;
num[2] = cnt;
num[3] = 0;
*/
MNET->Process();
} // while()
// for some reason, this does not work.
HW->SetAnalogOut(IoHardware::AMP_SIGNAL, 0.0);
HW->WriteDigitalBit(IoHardware::MOTOR_ENABLE, MOTOR_OFF);
HW->motorStatus = MOTOR_OFF;
delay(10);
FATAL_ERROR(err_msg);
}
/*******************************************************
* PWM test - check wiring and such
*******************************************************/
void PWM_Test(void)
{
init_pwm();
while (!(getkey() == 1))
{
for (int i = 0; i <= 255 - lmb_PARAM; i++) {
show12bits(i, i);
SetLeftMotorPWM(i);
SetRightMotorPWM(i);
TimerWait(500);
}
for (int i = 0; i <= 255 - lmb_PARAM; i++) {
show12bits(i, i);
SetLeftMotorPWM(-i);
SetRightMotorPWM(-i);
TimerWait(500);
}
/*
printLCD("LFWD");
SetLeftMotorPWM(255);
SetRightMotorPWM(0);
TimerWait(3000);
printLCD("LREV");
SetLeftMotorPWM(-255);
SetRightMotorPWM(0);
TimerWait(3000);
printLCD("RFWD");
SetLeftMotorPWM(0);
SetRightMotorPWM(255);
TimerWait(3000);
printLCD("RREV");
SetLeftMotorPWM(0);
SetRightMotorPWM(-255);
TimerWait(3000);
printLCD("2 FWD low");
SetRightMotorPWM(128);
SetLeftMotorPWM(128);
TimerWait(3000);
printLCD("2 REV med");
SetRightMotorPWM(-200);
SetLeftMotorPWM(-200);
TimerWait(3000);
printLCD("CCW");
SetRightMotorPWM(200);
SetLeftMotorPWM(-200);
TimerWait(3000);
printLCD("CW");
SetRightMotorPWM(-200);
SetLeftMotorPWM(200);
TimerWait(3000);
*/
}
// Done
SetLeftMotorPWM(0);
SetRightMotorPWM(0);
}
void GLWindow_Mainloop(void)
{
USE_HIGH_PERFORMANCE_TIMER = EmulatorConfig.highperformancetimer;
TimerInit();
SCREEN_TEXTURE = calloc(256*224,4); //max possible size
int scr_texture_loaded = 0;
GLuint scr_texture;
//do { GBA_RunFor(1); } while(GBA_MemoryReadFast16(CPU.R[R_PC]) != 0xDF05); //swi 0x05
//CPU.R[R_PC] = 0x00000000;
//do { GBA_RunFor(1); } while(CPU.R[R_PC] != 0x080002B0); GLWindow_GBACreateDissasembler();
while(1)
{
if(GLWindow_HandleEvents()) break;
if(GLWindow_Active && (PAUSED == 0))
{
if(RUNNING == RUN_GBA)
{
GLWindow_GBADisassemblerStartAddressSetDefault();
GLWindow_GBAHandleInput();
GBA_CheckKeypadInterrupt();
GBA_RunFor(280896); //clocksperframe = 280896
if(Keys_Down[VK_SPACE])
{
GBA_SetFrameskip(10);
GBA_SoundResetBufferPointers();
}
else GBA_SetFrameskip(FRAMESKIP);
if(GBA_HasToSkipFrame()==0)
{
GBA_ConvertScreenBufferTo32RGB(SCREEN_TEXTURE);
glClear(GL_COLOR_BUFFER_BIT); //Clear screen
if(scr_texture_loaded) glDeleteTextures(1,&scr_texture);
scr_texture_loaded = 1;
glGenTextures(1,&scr_texture);
glBindTexture(GL_TEXTURE_2D,scr_texture);
if(EmulatorConfig.oglfilter)
{
glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MIN_FILTER,GL_LINEAR);
glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MAG_FILTER,GL_LINEAR);
}
else
{
glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MIN_FILTER,GL_NEAREST);
glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MAG_FILTER,GL_NEAREST);
}
glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_WRAP_S,GL_CLAMP);
glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_WRAP_T,GL_CLAMP);
glTexImage2D(GL_TEXTURE_2D,0,4,240,160, 0,GL_RGBA,GL_UNSIGNED_BYTE,SCREEN_TEXTURE);
glBindTexture(GL_TEXTURE_2D,scr_texture);
glColor3f(1.0,1.0,1.0);
glBegin( GL_QUADS );
glTexCoord2f(0,0); // Top-left vertex
glVertex3f(0,0,0);
glTexCoord2f(1,0); // Bottom-left vertex
glVertex3f(240,0,0);
glTexCoord2f(1,1); // Bottom-right vertex
glVertex3f(240,160,0);
glTexCoord2f(0,1); // Top-right vertex
glVertex3f(0,160,0);
glEnd();
GLWindow_SwapBuffers();
}
GBA_UpdateFrameskip();
//GLWindow_MemViewerUpdate();
//GLWindow_IOViewerUpdate();
//GLWindow_DisassemblerUpdate();
TimerWait(Keys_Down[VK_SPACE] == 0);
}
else
{
glClear(GL_COLOR_BUFFER_BIT); //Clear screen
GLWindow_SwapBuffers();
Sleep(100);
}
}
else
{
Sleep(1); // Allow the CPU to rest a bit :P
}
/*
if(RUNNING == RUN_GBA)
{
if(Keys_Down['Q']) { int k = 10; while(k--) GLWindow_GBADisassemblerStep(); GLWindow_GBADisassemblerUpdate(); }
if(Keys_Down['W']) { int k = 100; while(k--) GLWindow_GBADisassemblerStep(); GLWindow_GBADisassemblerUpdate(); }
if(Keys_Down['R']) { int k = 1000; while(k--) GLWindow_GBADisassemblerStep(); GLWindow_GBADisassemblerUpdate(); }
}
*/
}
GLWindow_UnloadRom(1);
if(scr_texture_loaded) glDeleteTextures(1,&scr_texture);
TimerEnd();
}
static void *Spindown_Thread(void *arg)
{
struct TimerInfo info;
SpinDownState s_state = IDLE;
SpinDownData s_data[MAX_SPINDOWN_SAMPLES];
uint8_t s_index = 0;
uint8_t doSpinDown = 1;
// set resistance to 0
// Start the periodic timer @ 1s
TimerStart (1000000, &info);
// start the spindown process..
while(doSpinDown && currentMode == MODE_CALIBRATE)
{
// Wait for periodic timer to expire
TimerWait (&info);
double kph = currentSpeed;
switch (s_state)
{
case IDLE:
//printf("%.2f : ", kph);
//printf("Starting spindown calibration process...\n");
snprintf(DisplayMessage, DISPLAY_MAX_MSG_SIZE, "Starting spindown calibration process...");
// zero out the array of spindown entries & set index to start
memset(s_data, 0, sizeof (s_data));
s_index = 0;
s_state = WAIT;
break;
case WAIT:
if (kph > 50)
{
s_state = READY;
}
else
{
//printf("%.2f : ", kph);
//printf("Please increase speed to over 50 km/h...\n");
snprintf(DisplayMessage, DISPLAY_MAX_MSG_SIZE, "Please increase speed to over 50 km/h...");
}
break;
case READY:
if (kph > 50)
{
//printf("%.2f : ", kph);
//printf("Please stop pedaling to initiate calibration...\n");
snprintf(DisplayMessage, DISPLAY_MAX_MSG_SIZE, "Please stop pedaling to initiate calibration...");
}
else
{
s_state = RUNNING;
}
break;
case RUNNING:
//printf("%.2f : ", kph);
//printf("Calibrating, please wait....\n");
snprintf(DisplayMessage, DISPLAY_MAX_MSG_SIZE, "Calibrating, please wait....");
// get current speed & add to array
s_data[s_index].time = s_index+1;
s_data[s_index].speed = kph;
// check current speed is less than previous, else error
if (s_index > 0)
{
if (s_data[s_index].speed > s_data[s_index-1].speed)
{
s_state = ERROR;
break;
}
}
// have we reached enough samples, or low enough speed
if ((s_data[s_index].speed < 10) || (s_index == MAX_SPINDOWN_SAMPLES))
{
s_state = DONE;
break;
}
// ready for next sample
s_index++;
break;
case ERROR:
//printf("%.2f : ", kph);
//printf("Error, did you start pedaling? ;)...\n\n");
snprintf(DisplayMessage, DISPLAY_MAX_MSG_SIZE, "Error, did you start pedaling? ;)...");
s_state = IDLE;
break;
case DONE:
//printf("%.2f : ", kph);
//printf("Spindown calibration process successful...\n\n");
snprintf(DisplayMessage, DISPLAY_MAX_MSG_SIZE,
"Spindown successful: a = %lf, b = %lf, c = %lf, d = %lf",
power_curve_a, power_curve_b,
power_curve_c, power_curve_d);
//int i;
for (int i = 0; i < s_index; i++)
{
//printf("%.0f, %.2f\n", s_data[i].time, s_data[i].speed);
}
//printf("\n");
// ...and finish
doSpinDown = 0;
break;
default:
break;
}
if (s_state == DONE)
{
// Spindown completed, now fit data to curve
FitCurve(s_data, s_index);
}
if (currentMode != MODE_CALIBRATE)
{
//printf("Spindown aborted..\n");
snprintf(DisplayMessage, DISPLAY_MAX_MSG_SIZE, "Spindown aborted...");
}
}
currentMode = lastMode;
return NULL;
}
static void *ANT_Thread(void *arg)
{
struct TimerInfo info;
uint8_t power_event_count = 0;
uint16_t power_accumulated = 0;
uint16_t power_instant = 0;
double previous_ke = 0;
uint8_t power_accel_index;
double power_accel_array[POWER_SAMPLE_DEPTH];
double power_accel_filtered;
// zero out the array of power entries & set index to start
memset(power_accel_array, 0, sizeof (power_accel_array));
power_accel_index = 0;
// USB setup
if (libusb_init(NULL) != 0)
{
//printf("libusb: failed to initialise\n");
snprintf(DisplayMessage, DISPLAY_MAX_MSG_SIZE, "libusb: failed to initialise...");
return NULL;
}
devh = libusb_open_device_with_vid_pid(NULL, vendor_id, product_id);
if (devh == NULL)
{
//printf("libusb: failed to open device 0x%04x:0x%04x\n", vendor_id, product_id);
snprintf(DisplayMessage, DISPLAY_MAX_MSG_SIZE, "libusb: failed to open device 0x%04x:0x%04x\n", vendor_id, product_id);
}
else
{
// don't really care about the result..
libusb_detach_kernel_driver(devh, 0);
if (libusb_claim_interface(devh, 0) != 0)
{
//printf("libusb: failed to claim interface");
snprintf(DisplayMessage, DISPLAY_MAX_MSG_SIZE, "libusb: failed to claim interface");
}
else
{
//printf("libusb: succesfully claimed interface\n");
snprintf(DisplayMessage, DISPLAY_MAX_MSG_SIZE, "libusb: succesfully claimed interface");
// ANT+ setup
// reset
ANTBuildMessage(1, ANT_SYSTEM_RESET, 0, 0, 0, 0, 0, 0, 0, 0, 0);
ANTTransferMessage();
// ANT docs say wait 500ms after reset before sending any more host commands
milliSleep(500);
// set network key
ANTBuildMessage(9, ANT_SET_NETWORK, ANT_NETWORK_0, key[0], key[1], key[2], key[3], key[4], key[5], key[6], key[7]);
ANTTransferMessage();
// assign channel
ANTBuildMessage(3, ANT_ASSIGN_CHANNEL, ANT_CHANNEL_1, ANT_CHANNEL_TYPE_TX, ANT_NETWORK_0, 0, 0, 0, 0, 0, 0);
ANTTransferMessage();
// set channel id
uint16_t device_id = 0xBEEF;
ANTBuildMessage(5, ANT_CHANNEL_ID, ANT_CHANNEL_1, device_id&0xff, (device_id>>8)&0xff, ANT_SPORT_POWER_TYPE, ANT_TRANSMISSION_TYPE, 0, 0, 0, 0);
ANTTransferMessage();
// set rf frequency
ANTBuildMessage(2, ANT_CHANNEL_FREQUENCY, ANT_CHANNEL_1, ANT_SPORT_FREQUENCY, 0, 0, 0, 0, 0, 0, 0);
ANTTransferMessage();
// set channel period
uint16_t period = ANT_SPORT_POWER_PERIOD;
ANTBuildMessage(3, ANT_CHANNEL_PERIOD, ANT_CHANNEL_1, period&0xff, (period>>8)&0xff, 0, 0, 0, 0, 0, 0);
ANTTransferMessage();
// set tx power? (optional)
// open channel
ANTBuildMessage(1, ANT_OPEN_CHANNEL, ANT_CHANNEL_1, 0, 0, 0, 0, 0, 0, 0, 0);
ANTTransferMessage();
// Start the periodic timer @ 250ms
// todo: use stricter ANT power message interval?
TimerStart (250000, &info);
//TESTING!!!
// ONCE PER SECOND TO MATCH SPEED UPDATES
//TimerStart (1000000, &info);
// Run until terminated from main thread
while (runAntThread)
{
// Wait for periodic timer to expire
TimerWait (&info);
// Broadcast data
////printf("ANT_Thread: timer expired...\n");
// Data Page 0x10 message format
// Byte 0 : Data Page Number - 0x10
// Byte 1 : Update event count
// Byte 2 : Pedal power - 0xFF
// Byte 3 : Instantaneous cadence - 0xFF
// Byte 4 : Accumulated power (LSB)
// Byte 5 : Accumulated power (MSB)
// Byte 6 : Instantaneous power (LSB)
// Byte 7 : Instantaneous power (MSB)
// #define POWER 150
// power_instant = POWER;
double speed = currentSpeed;
// calculate the static power
double power_static = (power_curve_a +
(power_curve_b*speed) +
(power_curve_c*speed*speed) +
(power_curve_d*speed*speed*speed));
// calculate the kinetic energy at this speed
double speed_ms = speed / 3.6;
double current_ke = GetKineticEnergy(speed_ms);
// calculate the acceleration power. This calculation is dependent on the update
// frequency, as we are looking for the change in stored kinetic energy per second
//
double power_accel = (current_ke - previous_ke) * PRU_UPDATE_FREQ;
// todo: this may need some smoothing as the frequency increases?
// - start with a simple moving average of the acceleration power
power_accel_array[power_accel_index++] = power_accel;
if (power_accel_index >= POWER_SAMPLE_DEPTH)
{
power_accel_index = 0;
}
power_accel_filtered = 0;
for (int i = 0; i < POWER_SAMPLE_DEPTH; i++)
{
power_accel_filtered += power_accel_array[i];
}
power_accel_filtered /= POWER_SAMPLE_DEPTH;
//snprintf(DisplayMessage, DISPLAY_MAX_MSG_SIZE, "current_ke = %lf, previous_ke = %lf", total_kinetic_energy, previous_ke);
//snprintf(DisplayMessage, DISPLAY_MAX_MSG_SIZE, "power_static = %.0lf, power_accel = %.0lf", power_static, power_accel);
// Print out the value received from the PRU code
//printf("%u events, %.0f RPM, %.2f km/h, %.2f watts\r\n", events, rpm, kph, power_static+power_accel);
previous_ke = current_ke;
power_event_count++;
power_instant = power_static + power_accel_filtered;
currentPower = power_instant;
power_accumulated += power_instant;
ANTBuildMessage(9, ANT_BROADCAST_DATA, ANT_CHANNEL_1, ANT_STANDARD_POWER, power_event_count, 0xFF, 0xFF,
power_accumulated&0xff, (power_accumulated>>8)&0xff, power_instant&0xff, (power_instant>>8)&0xff);
ANTTransferMessage();
}
// ANT+ cleanup
//printf("ANT_Thread: cleanup\n");
// close channel
ANTBuildMessage(1, ANT_CLOSE_CHANNEL, ANT_CHANNEL_1, 0, 0, 0, 0, 0, 0, 0, 0);
ANTTransferMessage();
// USB cleanup
libusb_release_interface(devh, 0);
}
// USB cleanup
libusb_close(devh);
}
libusb_exit(NULL);
return NULL;
}
static void *Speed_Thread(void *arg)
{
struct TimerInfo info;
//return NULL;
// Initialise the PRU
//printf("Initialising PRU\n");
if (prussdrv_init() != 0)
{
snprintf(DisplayMessage, DISPLAY_MAX_MSG_SIZE, "prussdrv_init failed");
return NULL;
}
// Open an event
if (prussdrv_open(PRU_EVTOUT_0))
{
//printf("prussdrv_open failed\n");
snprintf(DisplayMessage, DISPLAY_MAX_MSG_SIZE, "prussdrv_open failed");
return NULL;
}
// Get pointers to PRU0 local memory
void *pruDataMem;
prussdrv_map_prumem (PRUSS0_PRU0_DATARAM, &pruDataMem);
unsigned int *pruData = (unsigned int *) pruDataMem;
// Execute code on PRU
//printf("Executing speed pru code\n");
if (prussdrv_exec_program (0, "../pru/BeagleTrainer.bin") < 0)
{
//printf("prussdrv_exec_program failed\n");
snprintf(DisplayMessage, DISPLAY_MAX_MSG_SIZE, "prussdrv_exec_program failed");
return NULL;
}
// Start the periodic timer @ 125ms
TimerStart (125000, &info);
// Run until terminated from main thread
while (runSpeedThread)
{
// Wait for periodic timer to expire
TimerWait (&info);
// Broadcast data
////printf("Speed Thread: timer expired...\n");
uint32_t events = pruData[0];
uint32_t frequency = events * PRU_UPDATE_FREQ;
double rps = (double)frequency / PULSE_PER_REV;
//double rpm = rps * 60.0;
double mps = rps * 2.0 * M_PI * RADIUS / 1000.0;
double kph = mps * 3.6;
currentSpeed = kph;
currentFrequency = events;
// Print out the value received from the PRU code
////printf("%u events, %.0f RPM, %.2f km/h\r\n", events, rpm, kph);
//snprintf(DisplayMessage, DISPLAY_MAX_MSG_SIZE, "%u events, %.0f RPM, %.2f km/h", events, rpm, kph);
}
// cleanup
//printf("Speed Thread: cleanup\n");
prussdrv_pru_disable(0);
prussdrv_exit();
return NULL;
}
