void Box2DWheelJoint::setEnableMotor(bool _enableMotor)
{
if (enableMotor() == _enableMotor)
return;
mWheelJointDef.enableMotor = _enableMotor;
if (mWheelJoint)
mWheelJoint->EnableMotor(_enableMotor);
emit enableMotorChanged();
}
/*lint -save -e970 Disable MISRA rule (6.3) checking. */
int main(void)
/*lint -restore Enable MISRA rule (6.3) checking. */
{
/* Write your local variable definition here */
uint16 countServo;
int i;
/*** Processor Expert internal initialization. DON'T REMOVE THIS CODE!!! ***/
PE_low_level_init();
ADC0_CFG2 |= ADC_CFG2_MUXSEL_MASK;
/*** End of Processor Expert internal initialization. ***/
/* Write your code here */
/* For example: for(;;) { } */
/*
* Motores são setados com valores de 0 a 255, 2o argumento resolve o sentido.
*/
cameraSIBit_ClrVal();
sMachine_Init();
Serial_Init();
enableMotor();
setMotor(0,1,0);
setMotor(1,1,0);
servoPWM_Enable(servoPWM_DeviceData);
countServo = 19000;
servoPWM_SetDutyUS(servoPWM_DeviceData,countServo);
cameraCLK_Enable();
cameraCLK_EnableEvent();
cameraCLKBit_ClrVal();
while(1){
for (i=0; i < 128; i++){
vetorCamera[i] = getCamPixel(i);
}
for (i=0; i < 128; i++){
itoa(vetorCamera[i], buffer0);
Send_String(buffer0);
Send_String(" ");
}
Send_String("\r\n");
}
/*** Don't write any code pass this line, or it will be deleted during code generation. ***/
/*** RTOS startup code. Macro PEX_RTOS_START is defined by the RTOS component. DON'T MODIFY THIS CODE!!! ***/
#ifdef PEX_RTOS_START
PEX_RTOS_START(); /* Startup of the selected RTOS. Macro is defined by the RTOS component. */
#endif
/*** End of RTOS startup code. ***/
/*** Processor Expert end of main routine. DON'T MODIFY THIS CODE!!! ***/
for(;;){}
/*** Processor Expert end of main routine. DON'T WRITE CODE BELOW!!! ***/
} /*** End of main routine. DO NOT MODIFY THIS TEXT!!! ***/
/**
"EM" — Enable Motors (for EBB hardware v1.1)
Command: EM,EnableAxis1[,EnableAxis2]<CR>
Response: OK<NL><CR>
Firmware versions: All
Execution: Immediate
Arguments: For each stepper motor (EnableAxis1 or EnableAxis2), an integer in
the range of 0 through 4, inclusive. The allowed values and their effects are as
follows:
0: Disable that axis
1: Enable in 1/8 step mode (default)
2: Enable in 1/4 step mode
3: Enable in 1/2 step mode
4: Enable in full step mode
Description:
Enable or disable stepper motors and set step size.
Each stepper motor axis may be enabled (energized) or disabled. When disabled,
the driver will stop sending current to the motor, so the motor will "freewheel"
— it will not be actively driven, but instead will present little resistance to
being turned by external torques.
When enabled, the stepper motor axes may be configured to be in whole, half,
quarter, or eighth step sizes. In a motor with a native resolution of 200 steps
per revolution, these settings would produce effective stepping resolutions of
200, 400, 800, and 1600 steps per revolution, respectively. Using fine sub-steps
("microstepping") gives higher resolution at the cost of decreasing step size
reproducibility.
Note that this version of the command is only for EBB hardware v1.1.
*/
void EBBParser::parseEM(const char* arg1, const char* arg2)
{
if (arg1 == NULL) {
sendError();
return;
}
sendAck();
const char* args[2] = { arg1, arg2 };
for (int axis = 0; axis < 2; ++axis) {
if (args[axis] != NULL) {
// ignore microstepping parameter since the inskape plugin assume 16 by default.
const bool state = atoi(args[axis]) != 0;
enableMotor(axis, state);
}
}
}
/*Enables (1) or disables (0) all motors.*/
void StepperControl::enableAll(int enable){
for(int i = 0; i < motorsCount; i++){
enableMotor(enable, i);
}
}
void AllConstraintDemo::clientMoveAndDisplay()
{
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
float dt = float(getDeltaTimeMicroseconds()) * 0.000001f;
//printf("dt = %f: ",dt);
// drive cone-twist motor
m_Time += 0.03f;
if (s_bTestConeTwistMotor)
{ // this works only for obsolete constraint solver for now
// build cone target
btScalar t = 1.25f*m_Time;
btVector3 axis(0,sin(t),cos(t));
axis.normalize();
btQuaternion q1(axis, 0.75f*SIMD_PI);
// build twist target
//btQuaternion q2(0,0,0);
//btQuaternion q2(btVehictor3(1,0,0), -0.3*sin(m_Time));
btQuaternion q2(btVector3(1,0,0), -1.49f*btSin(1.5f*m_Time));
// compose cone + twist and set target
q1 = q1 * q2;
m_ctc->enableMotor(true);
m_ctc->setMotorTargetInConstraintSpace(q1);
}
{
static bool once = true;
if ( m_dynamicsWorld->getDebugDrawer() && once)
{
m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawConstraints+btIDebugDraw::DBG_DrawConstraintLimits);
once=false;
}
}
{
//during idle mode, just run 1 simulation step maximum
int maxSimSubSteps = m_idle ? 1 : 1;
if (m_idle)
dt = 1.0f/420.f;
int numSimSteps = m_dynamicsWorld->stepSimulation(dt,maxSimSubSteps);
//optional but useful: debug drawing
m_dynamicsWorld->debugDrawWorld();
bool verbose = false;
if (verbose)
{
if (!numSimSteps)
printf("Interpolated transforms\n");
else
{
if (numSimSteps > maxSimSubSteps)
{
//detect dropping frames
printf("Dropped (%i) simulation steps out of %i\n",numSimSteps - maxSimSubSteps,numSimSteps);
} else
{
printf("Simulated (%i) steps\n",numSimSteps);
}
}
}
}
renderme();
// drawLimit();
glFlush();
swapBuffers();
}
//-----------------------------------------------
//local functions
//-----------------------------------------------
void execEvent(struct Event event)
{
int i, iPara, iCmd;
int iSynchVal, iSynchValMeasNorm, iDeltaSynchMeas;
static enum STATE_Connection s_StConnection = ST_ConnReset;
static enum STATE_Motor s_StMotor = ST_BrakeClosed;
iCmd = event.ucData[0];
switch(s_StConnection)
{
case ST_ConnReset:
//-------------------
switch(iCmd)
{
//-------------------
case CMD_MOTCTRL_CONNECT:
transmitMsg(event.iSource, 0, 0, 0, 0, 0, 0, 0, (iCmd << 2) | Msg_OK);
s_StConnection = ST_ConnConnected;
break;
//-------------------
case CMD_MOTCTRL_DISCONNECT:
_trap_(0x00);
break;
//-------------------
default:
if(event.iSource != EVSOURCE_INTERN)
transmitMsg(event.iSource, 0, 0, 0, 0, 0, 0, 0, (iCmd << 2) | Msg_NotAccept);
break;
}
break; //case ST_ConnReset
case ST_ConnConnected:
//---------------------------------------
//commands always executed
switch(iCmd)
{
//-------------------
case CMD_MOTCTRL_DISCONNECT:
_trap_(0x00);
return;
//-------------------
//io
case CMD_MOTCTRL_SETDIGOUT:
setOutputPort8(event.ucData[1], event.ucData[2]);
return;
//-------------------
case CMD_MOTCTRL_GETDIGIN:
transmitMsg(event.iSource, P1H, P1L, 0, 0, 0, 0, 0, (iCmd << 2) | Msg_OK);
return;
//-------------------
case CMD_MOTCTRL_GETANALOGIN:
iPara = g_iAnIn[event.ucData[1]];
transmitMsg(event.iSource, iPara >> 8, iPara, 0, 0, 0, 0, 0, (iCmd << 2) | Msg_OK);
return;
//-------------------
//debug
case CMD_MOTCTRL_SETCTRLPARA:
g_iIn1 = (event.ucData[1] << 8) | event.ucData[2];
g_iIn2 = (event.ucData[3] << 8) | event.ucData[4];
g_iIn3 = (event.ucData[5] << 8) | event.ucData[6];
//synch pos fine tuning
//gc_iFieldPosSynchNorm = g_iIn1;
return;
//-------------------
case CMD_MOTCTRL_GETCTRLPARA:
transmitMsg(event.iSource, g_iOut1 >> 8, g_iOut1, g_iOut2 >> 8, g_iOut2, g_iOut3 >> 8, g_iOut3, 0, (iCmd << 2) | Msg_OK);
return;
//-------------------
//get data
case CMD_MOTCTRL_GETPOSVEL:
transmitMsg(event.iSource, g_lPosMeas >> 24, g_lPosMeas >> 16, g_lPosMeas >> 8, g_lPosMeas, g_iVelMeas >> 8, g_iVelMeas, 0, (iCmd << 2) | Msg_OK);
return;
//-------------------
case CMD_MOTCTRL_GETSTATUS:
transmitMsg(event.iSource, g_iStateCtrl >> 8, g_iStateCtrl, g_iTempMeas >> 8, g_iTempMeas, 0, 0, 0, (iCmd << 2) | Msg_OK);
return;
}
//---------------------------------------
//motor control commands state machine
switch(s_StMotor)
{
//-------------------
case ST_BrakeClosed:
switch(iCmd)
{
case CMD_MOTCTRL_DISABLEBRAKE:
if(event.ucData[1] == 1)
{
openBrake(gc_iBrakeHasRelais);
transmitMsg(event.iSource, 0, 0, 0, 0, 0, 0, 0, (iCmd << 2) | Msg_OK);
s_StMotor = ST_BrakeOpen;
}
break; //CMD_MOTCTRL_DISABLEBRAKE
default:
if(event.iSource != EVSOURCE_INTERN)
transmitMsg(event.iSource, 0, 0, 0, 0, 0, 0, 0, (iCmd << 2) | Msg_NotAccept);
} //switch(iCmd)
break; //ST_BrakeClosed
//-------------------
case ST_BrakeOpen:
switch(iCmd)
{
case CMD_MOTCTRL_DISABLEBRAKE:
if(event.ucData[1] == 0)
{
closeBrake(gc_iBrakeHasRelais);
s_StMotor = ST_BrakeClosed;
transmitMsg(event.iSource, 0, 0, 0, 0, 0, 0, 0, (iCmd << 2) | Msg_OK);
}
break; //CMD_MOTCTRL_DISABLEBRAKE
case CMD_MOTCTRL_ENABLEMOTOR:
if(event.ucData[1] == 1)
{
enableMotor(gc_iSineZero);
g_iPosVelCtrlRun = 0;
g_iTorqueCmd = 0;
g_iTorqueCtrlRun = 1;
g_iFieldPos = 0;
g_iCommRun = 0;
g_iSynchRun = 1;
//increase torque slowly
do
{
g_iTorqueCmd++;
Delay(5000);
}
while(g_iTorqueCmd < gc_iSynchTorque);
s_StMotor = ST_MotorEnabled;
transmitMsg(event.iSource, 0, 0, 0, 0, 0, 0, 0, (iCmd << 2) | Msg_OK);
}
break; //CMD_MOTCTRL_ENABLEMOTOR
default:
if(event.iSource != EVSOURCE_INTERN)
transmitMsg(event.iSource, 0, 0, 0, 0, 0, 0, 0, (iCmd << 2) | Msg_NotAccept);
} //switch(iCmd)
break; //ST_BrakeOpen
//-------------------
case ST_MotorEnabled:
switch(iCmd)
{
case CMD_MOTCTRL_ENABLEMOTOR:
if(event.ucData[1] == 0)
{
disableMotorCtrl();
s_StMotor = ST_BrakeOpen;
transmitMsg(event.iSource, 0, 0, 0, 0, 0, 0, 0, (iCmd << 2) | Msg_OK);
}
break; //CMD_MOTCTRL_ENABLEMOTOR
case CMD_MOTCTRL_SYNCHMOTOR:
//exec single rot to snap in rotor
for(i=0; i<gc_iEncPulsePerRev; i++)
{
g_iFieldPos++;
Delay(gc_iSynchVelDelay);
}
g_iFieldPos = 0;
g_lPosOffset = g_lPosOffset + T2; //g_lPosOffset used to move back to 0 after synch
T2 = 0;
//enable synch int
g_iSynchIntCaptured = 0;
EXICON = 0x0008 | 0x0001; //EXIN0 interrupt on pos edge, EXIN1 interrupt on neg edge
//wait for synch interrupt, communication blocking
while(g_iSynchIntCaptured == 0)
{
_atomic_(3);
//if no synch interrupt captured, update iSynchVal
if(g_iSynchIntCaptured == 0)
iSynchVal = T2;
g_iFieldPos++;
Delay(gc_iSynchVelDelay);
}
g_lPosOffset = g_lPosOffset + iSynchVal;
//shift back iSynchVal
while(iSynchVal > gc_iEncPulsePerPhase)
iSynchVal = iSynchVal - gc_iEncPulsePerPhase;
//reset T2, g_iT2RevCnt -> EncPosAbs = 0
T2 = 0;
g_iT2RevCnt = 0;
if(gc_iMotorType == MotorType_6Pol)
iSynchValMeasNorm = toFieldPosNorm_Mot6Pol_Enc4096(iSynchVal);
if(gc_iMotorType == MotorType_4Pol)
iSynchValMeasNorm = toFieldPosNorm_Mot4Pol_Enc4096(iSynchVal);
iDeltaSynchMeas = iSynchValMeasNorm - gc_iFieldPosSynchNorm;
if(iDeltaSynchMeas < -255)
iDeltaSynchMeas = iDeltaSynchMeas + 512;
if(iDeltaSynchMeas > 255)
iDeltaSynchMeas = iDeltaSynchMeas - 512;
//test for correct SynchVal
if((iDeltaSynchMeas > -30) && (iDeltaSynchMeas < 90))
{
transmitMsg(event.iSource, iSynchValMeasNorm >> 8, iSynchValMeasNorm, gc_iFieldPosSynchNorm >> 8, gc_iFieldPosSynchNorm, 0, 0, 0, (iCmd << 2) | Msg_OK);
s_StMotor = ST_MotorSynch;
}
else
{
disableMotorCtrl();
transmitMsg(event.iSource, iSynchValMeasNorm >> 8, iSynchValMeasNorm, gc_iFieldPosSynchNorm >> 8, gc_iFieldPosSynchNorm, 0, 0, 0, (iCmd << 2) | Msg_Error);
s_StMotor = ST_BrakeOpen;
}
break; //CMD_MOTCTRL_SYNCHMOTOR
default:
if(event.iSource != EVSOURCE_INTERN)
transmitMsg(event.iSource, 0, 0, 0, 0, 0, 0, 0, (iCmd << 2) | Msg_NotAccept);
} //switch(iCmd)
break; //ST_MotorEnabled
//-------------------
case ST_MotorSynch:
switch(iCmd)
{
case CMD_MOTCTRL_ENABLECOMM:
g_iTorqueCmd = 0;
while(g_lTorqueCmd != 0)
;
g_iT2Overflow = 0;
g_lPosCmd = getEncPosAbsAsLong();
g_iSynchRun = 0;
g_iCommRun = 1;
g_iPosVelCtrlRun = 1;
s_StMotor = ST_MotorRunning;
setVoltDownBrake(gc_iBrakeHasRelais);
transmitMsg(event.iSource, 0, 0, 0, 0, 0, 0, 0, (iCmd << 2) | Msg_OK);
break; //CMD_MOTCTRL_ENABLECOMM
default:
if(event.iSource != EVSOURCE_INTERN)
transmitMsg(event.iSource, 0, 0, 0, 0, 0, 0, 0, (iCmd << 2) | Msg_NotAccept);
} //switch
break; //ST_MotorSynch
//-------------------
case ST_MotorRunning:
switch(iCmd)
{
case CMD_MOTCTRL_SETCMDVAL:
g_lPosCmd = (((long int)event.ucData[1]) << 24 | ((long int)event.ucData[2]) << 16 | ((long int)event.ucData[3]) << 8 | (long int)event.ucData[4]);
g_iVelCmd = event.ucData[5] << 8 | event.ucData[6];
transmitMsg(event.iSource, g_iStateCtrl >> 8, g_iStateCtrl, g_iTempMeas >> 8, g_iTempMeas, 0, 0, 0, (CMD_MOTCTRL_GETSTATUS << 2) | Msg_OK);
transmitMsg(event.iSource, g_lPosMeas >> 24, g_lPosMeas >> 16, g_lPosMeas >> 8, g_lPosMeas, g_iVelMeas >> 8, g_iVelMeas, 0, (CMD_MOTCTRL_GETPOSVEL << 2) | Msg_OK);
break; //CMD_MOTCTRL_SETCMDVAL
case CMD_MOTCTRL_SETMOTIONTYPE:
g_iStMotionType = event.ucData[1];
transmitMsg(event.iSource, 0, 0, 0, 0, 0, 0, 0, (iCmd << 2) | Msg_OK);
break; //CMD_MOTCTRL_SETMOTIONTYPE
case CMD_MOTCTRL_SETEMSTOP:
g_iPosVelCtrlRun = 0;
g_iVelCmd = 0;
g_iTorqueCmd = 0;
//decrease torque slowly
/*
while(g_iTorqueCmd > 0)
{
g_iTorqueCmd--;
Delay(3000);
}
*/
closeBrake(gc_iBrakeHasRelais);
s_StMotor = ST_MotorEMStop;
if(event.iSource != EVSOURCE_INTERN)
transmitMsg(event.iSource, 0, 0, 0, 0, 0, 0, 0, (iCmd << 2) | Msg_OK);
break; //CMD_MOTCTRL_SETEMSTOP
case CMD_MOTCTRL_ENABLEMOTOR:
if(event.ucData[1] == 0)
{
disableMotorCtrl();
s_StMotor = ST_BrakeOpen;
transmitMsg(event.iSource, 0, 0, 0, 0, 0, 0, 0, (iCmd << 2) | Msg_OK);
}
break; //CMD_MOTCTRL_ENABLEMOTOR
default:
if(event.iSource != EVSOURCE_INTERN)
transmitMsg(event.iSource, 0, 0, 0, 0, 0, 0, 0, (iCmd << 2) | Msg_NotAccept);
} //switch(iCmd)
break; //ST_MotorRunning
//-------------------
case ST_MotorEMStop:
switch(iCmd)
{
case CMD_MOTCTRL_SETCMDVAL:
transmitMsg(event.iSource, g_iStateCtrl >> 8, g_iStateCtrl, g_iTempMeas >> 8, g_iTempMeas, 0, 0, 0, (CMD_MOTCTRL_GETSTATUS << 2) | Msg_OK);
transmitMsg(event.iSource, g_lPosMeas >> 24, g_lPosMeas >> 16, g_lPosMeas >> 8, g_lPosMeas, g_iVelMeas >> 8, g_iVelMeas, 0, (CMD_MOTCTRL_GETPOSVEL << 2) | Msg_OK);
break; //CMD_MOTCTRL_SETCMDVAL
case CMD_MOTCTRL_RESETEMSTOP:
openBrake(gc_iBrakeHasRelais);
g_iPosVelCtrlRun = 1;
s_StMotor = ST_MotorRunning;
transmitMsg(event.iSource, 0, 0, 0, 0, 0, 0, 0, (iCmd << 2) | Msg_OK);
break;
case CMD_MOTCTRL_ENABLEMOTOR:
if(event.ucData[1] == 0)
{
disableMotorCtrl();
s_StMotor = ST_BrakeOpen;
transmitMsg(event.iSource, 0, 0, 0, 0, 0, 0, 0, (iCmd << 2) | Msg_OK);
}
break; //CMD_MOTCTRL_ENABLEMOTOR
default:
if(event.iSource != EVSOURCE_INTERN)
transmitMsg(event.iSource, 0, 0, 0, 0, 0, 0, 0, (iCmd << 2) | Msg_NotAccept);
} //switch(iCmd)
break; //ST_MotorEMStop
} //switch(StMotor)
