void SimpleHome(int axis,float speed,int dir,int bit,int polarity, float offset)
{
int SaveLimits; //place to save limit switch settings
DisableAxis(axis);
// disable the limits (first save how they were set)
SaveLimits = chan[axis]->LimitSwitchOptions;
chan[axis]->LimitSwitchOptions = 0;
EnableAxis(axis); // enable axis and begin servoing where we are
// Home - jog until it sees the limit
Jog(axis,speed*dir); // jog slowly
while (!ReadBit(bit)) ; // loop until IO bit goes high
Jog(axis,0); // stop
while (!CheckDone(axis)) // loop until motion completes
if (!chan[axis].Enable) return; // abort/exit if diasabled
DisableAxis(axis); // disable the axis
Zero(axis); // Zero the position
EnableAxis(axis); // re-enable
Move(axis,-dir * offset); // move some amount inside the limits
while (!CheckDone(axis)) ; // loop until motion completes
chan[axis]->LimitSwitchOptions = SaveLimits; // restore limit settings
}
//用于查询外部按键状态
void KeysHandler_Task(void *Task_Parameters )
{
GPIO_InitTypeDef GPIO_InitStructure;
u32 KeyMap=0;
u8 i;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
for(i=0;i<EXTI_KEY_MAX_NUM;i++)
{
RCC_APB2PeriphClockCmd(gExtiKeyDefine[i].RccId,ENABLE);
GPIO_InitStructure.GPIO_Pin = gExtiKeyDefine[i].GpioPin;
GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = gExtiKeyDefine[i].GpioMode;
GPIO_Init(gExtiKeyDefine[i].GpioGroup, &GPIO_InitStructure);
}
while(1)
{
for(i=0;i<EXTI_KEY_MAX_NUM;i++)
{
if(GPIO_ReadInputDataBit(gExtiKeyDefine[i].GpioGroup,gExtiKeyDefine[i].GpioPin)!=ReadBit(KeyMap,i))//有变化
{
KeyMap^=(1<<(i));
if(gExtiKeyDefine[i].GpioMode == GPIO_Mode_IPD)//下拉输入
ExtiKeyHandler(i,GPIO_ReadInputDataBit(gExtiKeyDefine[i].GpioGroup,gExtiKeyDefine[i].GpioPin));
else if(gExtiKeyDefine[i].GpioMode == GPIO_Mode_IPU)//上拉输入
ExtiKeyHandler(i,!GPIO_ReadInputDataBit(gExtiKeyDefine[i].GpioGroup,gExtiKeyDefine[i].GpioPin));
LCD_Light_Counter=0;
}
}
OS_TaskDelayMs(100);
}
}
void secp256k1_ecmult_gen3(secp256k1_gej_t *r, const unsigned char *seckey){
unsigned char a [256];
for (int j = 0 ; j < 32; j++){
for (int i = 0 ; i < 8 ; i ++ ){
a[i+j*8] = ReadBit(seckey[31-j],i);
}
}
r->infinity = 1;
int bits;
for (int j = 0; j < numberOfWindows; j++) {
if (j == numberOfWindows -1 && remmining != 0){
bits = 0;
for (int i = 0; i < remmining; i++){
SetBit(bits,i,a[i + j * WINDOW_SIZE]);
}
}else{
bits = 0;
for (int i = 0; i < WINDOW_SIZE; i++){
SetBit(bits,i,a[i + j * WINDOW_SIZE]);
}
}
secp256k1_gej_add_ge(r, r, &prec[j*numberOfValues + bits]);
}
}
void secp256k1_ecmult_gen_bl(secp256k1_gej_t *r, const unsigned char *seckey){
unsigned char a [256];
for (int j = 0 ; j < 32; j++){
for (int i = 0 ; i < 8 ; i ++ ){
a[i+j*8] = ReadBit(seckey[31-j],i);
}
}
r->infinity = 1;
int bits;
for (int j = 0; j < numberOfWindows; j++) {
if (j == numberOfWindows -1 && remmining != 0){
bits = 0;
for (int i = 0; i < remmining; i++){
SetBit(bits,i,a[i + j * WINDOW_SIZE]);
}
//bits = secp256k1_scalar_get_bits2(a, j * WINDOW_SIZE, remmining);
}else{
bits = 0;
for (int i = 0; i < WINDOW_SIZE; i++){
SetBit(bits,i,a[i + j * WINDOW_SIZE]);
}
//bits = secp256k1_scalar_get_bits2(a, j * WINDOW_SIZE, WINDOW_SIZE);
}
secp256k1_gej_add_ge_bl(r, r, &prec[j*numberOfValues + bits],NULL);
}
//printf("%d\n",b);
}
//�鿴ϵͳ�¼�
//���RegIDΪ0�����ص�ǰҳ����¼���־
INSPECT_SYSEVT_RET Q_InspectPeripEvt(PAGE_RID RegID,PERIP_EVT PeripEvt)
{
u8 PageIdx;
if(RegID)
{
if(FindPage("",RegID,&PageIdx)!=SM_State_OK) return NoHasSysEvt;//�˴�״̬��ʧ��
}
else //���RegIDΪ0�����ص�ǰҳ����¼���־
{
PageIdx=GetPageIdxByTrack(0);
}
if(ReadBit(gPagePeripEvtFlag[PageIdx],PeripEvt)) return HasPagePeripEvt;
if(ReadBit(gGobalPeripEvtBitFlag,PeripEvt)) return HasGobalSysEvt;
return NoHasSysEvt;
}
/** Get sleep mode status.
* Setting the SLEEP bit in the register puts the device into very low power
* sleep mode. In this mode, only the serial interface and internal registers
* remain active, allowing for a very low standby current. Clearing this bit
* puts the device back into normal mode. To save power, the individual standby
* selections for each of the gyros should be used if any gyro axis is not used
* by the application.
* @return Current sleep mode enabled status
* @see MPU6050_RA_PWR_MGMT_1
* @see MPU6050_PWR1_SLEEP_BIT
*/
bool clMPU6050::GetSleepModeStatus()
{
uint8_t tmp;
ReadBit(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, &tmp);
if(tmp == 0x00)
return false;
else
return true;
}
Int32 SWFReader::ReadSB(int nBits) {
Int32 result = 0;
// Is there anything to read?
if (nBits > 0) {
// Is this a negative number (MSB will be set)?
if (ReadBit()) {
result -= (Int32) bitValues[nBits - 1];
}
// Calculate rest of value
for (int index = nBits - 2; index > -1; index--) {
if (ReadBit()) {
result |= (Int32) bitValues[index];
}
}
}
return result;
}
main()
{
for (;;) // loop forever
{
WaitNextTimeSlice();
if (ReadBit(46)) // Watch an external input switch
{
StopCoordinatedMotion(); //feedhold
}
}
}
int BitInputStream::ReadBits(int num)
{
if ((num < 0) || (num > 32)) {
throw new jcommon::OutOfBoundsException("Number of bits is out of range");
}
int bits = 0;
for (int i=0; i<num; i++) {
bits = (bits << 1) | ReadBit();
}
return bits;
}
void DoSpeedAxis(int ch, int Select, double *LastSpeed, double Speed)
{
if (ReadBit(Select)) //is this axis selected ??
{
Jog(ch,Speed);
*LastSpeed = Speed;
T0=T1; // save last time speed was changed
}
else if (*LastSpeed != 0.0) //not selected so stop if was moving
{
Jog(ch,0.0);
*LastSpeed=0.0;
}
}
UInt32 SWFReader::ReadUB(int nBits) {
UInt32 result = 0;
// Is there anything to read?
if (nBits > 0) {
// Calculate value
for (int index = nBits - 1; index > -1; index--) {
if (ReadBit()) {
result |= bitValues[index];
}
}
}
return result;
}
//发送读指令到下位机,发送站点默认为01
//type：功能号
//address：起始地址
//count：长度
void modbusRTU::SendReadCommand(unsigned char type,Int16 address,Int16 count)
{
switch(type)
{
case 1: // 01功能码：读取线圈（输出）状态 读取一组逻辑线圈的当前状态（ON/OFF）
ReadBit(type,address,count);
break;
case 2: //02功能码：读取输入状态 读取一组开关输入的当前状态（ON/OFF）
ReadBit(type,address,count);
break;
case 3: //03功能码：读取保持型寄存器 在一个或多个保持寄存器中读取当前二进制值
ReadReg(type,address,count);
break;
case 4: //04功能码：读取输入寄存器 在一个或多个输入寄存器中读取当前二进制值
ReadReg(type,address,count);
break;
default:
break;
}
rec_stat = PACK_START;
}
// Read unsigned integer Exp-Golomb-coded.
uint32_t ReadUE()
{
uint32_t i = 0;
while (ReadBit() == 0 && i < 32) {
i++;
}
if (i == 32) {
MOZ_ASSERT(false);
return 0;
}
uint32_t r = ReadBits(i);
r += (1 << i) - 1;
return r;
}
int CheckForOneButtonPushed(void)
{
int i,b,n=0;
for (i=0; i<NUM_TOOL_BUTTONS; i++)
{
if (ReadBit(FIRST_TOOL_BUTTON+i))
{
b=i; // rememeber last one
n++; // remember how many
}
}
if (n==1) return b; // one pushed, return which one
if (n==0) return -1; // none pushed, return -1
return -2; // more than one, return -2
}
bool BufferedReader::ReadByte(unsigned char& val) {
val = 0;
for(size_t i = 0; i < kByteSize; i++) {
bool bit = false;
if(ReadBit(bit)) {
if(bit) {
val |= (1 << i);
}
} else {
return false;
}
}
return true;
}
// -----------------------------------------------------------------------------
// TJ2kStreamReader::ReadBits
// Read some bits from the packet header stream.
// (other items were commented in a header).
// -----------------------------------------------------------------------------
//
TUint8 TJ2kStreamReader::ReadBits( TUint32& aBit, TUint8 aBitLen )
{
aBit = (TUint32)0;
TUint8 bit = 0;
for ( TInt8 index = ( TInt8 )( aBitLen - 1 ); index >= 0; --index )
{
if ( !ReadBit( bit ) )
{
aBit |= ( bit << index );
}
else
{
return ETrue;
}
}
return EFalse;
}
// Read unsigned integer Exp-Golomb-coded.
uint32_t ReadUE()
{
uint32_t i = 0;
while (ReadBit() == 0 && i < 32) {
i++;
}
if (i == 32) {
// This can happen if the data is invalid, or if it's
// short, since ReadBit() will return 0 when it runs
// off the end of the buffer.
NS_WARNING("Invalid H.264 data");
return 0;
}
uint32_t r = ReadBits(i);
r += (1 << i) - 1;
return r;
}
//处理全局事件响应
SYS_MSG MyGobalPeripEvtHandler(PERIP_EVT PeripEvt,int intParam, void *pParam)
{
SYS_MSG SysMsg=0;
u8 i;
if(!ReadBit(gGobalPeripEvtBitFlag,PeripEvt)) return 0;//没有此类事件
//找到匹配记录
for(i=0;i<MAX_GOBAL_SYSEVT;i++)
{
if(gGobalPeripEvtRecord[i].PeripEvt==PeripEvt)//事件对上号了
{
SysMsg|=gGobalPeripEvtRecord[i].GobalPeripEvtHandler(PeripEvt,intParam,pParam);
}
}
return SysMsg;
}
u32 BitStream::ReadBits(int count)
{
u8 data[4] = { 0 };
int cur_byte = 0;
int cur_bit = 0;
assert(num_bits_in_elem_ == 8);
int total_bits = std::min(num_bits_in_elem_, count);
while(count-- > 0)
{
if (ReadBit()) ///\note Can optimize here - read several bits at a time instead of looping bit-per-bit.
data[cur_byte] |= 1 << (total_bits - 1 - cur_bit);
if (++cur_bit >= num_bits_in_elem_)
{
++cur_byte;
cur_bit = 0;
total_bits = std::min(num_bits_in_elem_, count);
}
}
return *reinterpret_cast<u32*>(data);
}
void ExpandFile ( BFILE *input, FILE *output)
{
int i;
int current_pos;
int c;
int match_len;
int match_pos;
current_pos = 1;
for (;;)
{
if (ReadBit (input))
{
c = (int) ReadBits ( input, 8);
putc ( c, output);
window [current_pos] = (uchar) c;
current_pos = MODULO (current_pos + 1);
}
else
{
match_pos = (int) ReadBits ( input, INDEX_BITS);
if (match_pos == END_OF_STREAM)
break;
match_len = (int) ReadBits ( input, LENGTH_BITS);
match_len += BREAK_EVEN;
for ( i = 0; i <= match_len; i++)
{
c = window [MODULO (match_pos + i)];
putc ( c, output);
window [current_pos] = (uchar) c;
current_pos = MODULO (current_pos + 1);
}
}
}
}
// Services Tool Change Sequence
void ServiceToolChange(void)
{
switch (*ChangerState)
{
case T_IDLE:
{
break;
}
case T_START:
{
if (*LastTool==0)
{
printf("Error Last Tool Never defined\n");
*ChangerState = T_IDLE; // go idle
}
else if (*LastTool > 5 || *LastTool < 1)
{
printf("Invalid LastTool Number %d\n",*Tool);
*ChangerState = T_IDLE; // go idle
}
else if (*Tool > 5 || *Tool < 1)
{
printf("Invalid Tool Number %d\n",*Tool);
*ChangerState = T_IDLE; // go idle
}
else
{
printf("Tool Change Z Up\n");
Move(Z,10000);
*ChangerState = T_WAIT_Z_UP;
}
break;
}
case T_WAIT_Z_UP:
{
if (CheckDone(Z))
{
printf("Tool Change Move XY to Unload\n");
Move(X,ToolPositionX[*LastTool]);
Move(Y,ToolPositionY[*LastTool]);
*ChangerState = T_WAIT_MOVE_XY_UNLOAD;
}
break;
}
case T_WAIT_MOVE_XY_UNLOAD:
{
if (CheckDone(X) && CheckDone(Y))
{
printf("Tool Change Z Down\n");
Move(Z,10000); // start Z moving down fast
*ChangerState = T_WAIT_MOVE_Z_DOWN_FAST;
}
break;
}
case T_WAIT_MOVE_Z_DOWN_FAST:
{
if (chan[Z].Dest < 1000.0)
{
MoveAtVel(Z,10000,100.0); // slow down Z
*ChangerState = T_WAIT_MOVE_Z_DOWN_SLOW;
}
break;
}
case T_WAIT_MOVE_Z_DOWN_SLOW:
{
if (chan[Z].Dest < 500.0)
{
MoveAtVel(Z,10000,50.0); // slow down Z more
*ChangerState = T_WAIT_MOVE_Z_DOWN_VERYSLOW;
}
break;
}
case T_WAIT_MOVE_Z_DOWN_VERYSLOW:
{
if (CheckDone(Z))
{
printf("Tool Change Unclamp\n");
SetBit(TOOL_CLAMP_BIT);
*ChangerState = T_WAIT_OPENCLAMP;
}
break;
}
case T_WAIT_OPENCLAMP:
{
if (ReadBit(TOOL_CLAMP_STATUS_BIT))
{
printf("Tool Change Z Up #2\n");
Move(Z,10000);
*ChangerState = T_WAIT_Z_UP2;
}
break;
}
case T_WAIT_Z_UP2:
{
if (CheckDone(Z))
{
printf("Tool Change Move XY to Load\n");
Move(X,ToolPositionX[*Tool]);
Move(Y,ToolPositionY[*Tool]);
*ChangerState = T_WAIT_MOVE_XY_LOAD;
}
break;
}
case T_WAIT_MOVE_XY_LOAD:
{
if (CheckDone(X) && CheckDone(Y))
{
printf("Tool Change Z Down\n");
Move(Z,10000); // start Z moving down fast
*ChangerState = T_WAIT_MOVE_Z_DOWN_FAST2;
}
break;
}
case T_WAIT_MOVE_Z_DOWN_FAST2:
{
if (chan[Z].Dest < 1000.0)
{
MoveAtVel(Z,10000,100.0); // slow down Z
*ChangerState = T_WAIT_MOVE_Z_DOWN_SLOW2;
}
break;
}
case T_WAIT_MOVE_Z_DOWN_SLOW2:
{
if (chan[Z].Dest < 500.0)
{
MoveAtVel(Z,10000,50.0); // slow down Z more
*ChangerState = T_WAIT_MOVE_Z_DOWN_VERYSLOW2;
}
break;
}
case T_WAIT_MOVE_Z_DOWN_VERYSLOW2:
{
if (CheckDone(Z))
{
SetBit(TOOL_CLAMP_BIT);
*ChangerState = T_WAIT_CLOSECLAMP;
}
break;
}
case T_WAIT_CLOSECLAMP:
{
if (!ReadBit(TOOL_CLAMP_STATUS_BIT))
{
printf("Tool Change Z Up #2\n");
Move(Z,10000);
*ChangerState = T_WAIT_Z_UP3;
}
break;
}
case T_WAIT_Z_UP3:
{
if (CheckDone(Z))
{
printf("Tool Change Move XY to Probe\n");
Move(X,ProbeX);
Move(Y,ProbeY);
*ChangerState = T_WAIT_MOVE_XY_PROBE;
}
break;
}
case T_WAIT_MOVE_XY_PROBE:
{
if (CheckDone(X) && CheckDone(Y))
{
printf("Tool Change Complete\n");
*LastTool = *Tool; // remember where we are
*ChangerState = T_IDLE;
}
break;
}
}
}
void main()
{
float mid,k=0,dk=0.2,A=AMPLITUDE; // set coil current amplitude
int i,ignore=300,kpos[Ncycles],zmark,m=0;
double cnts_per_cycle,p0[Ncycles];
CHAN *ch = &chan[AXIS_CHAN];
// rotate until we find the index mark
ch->Enable=FALSE;
ch->InputMode=ENCODER_MODE;
ch->InputChan0=ENCODER_CHAN;
ch->OutputChan0=DAC_CHAN;
ch->OutputChan1=DAC_CHAN+1;
ch->OutputMode=NO_OUTPUT_MODE;
ch->InputGain0=ENCODER_GAIN;
for (;;)
{
WaitNextTimeSlice();
k+= dk;
Write3PH_DACs(ch,A, k/10000.0); // move the pole
zmark = ReadBit(Z_BIT_NUMBER);
if (!zmark && ignore>0) ignore--;
if (ignore==0 && zmark) // check for index mark
{
p0[m]=ch->Position; // save position
kpos[m]=k; // save phase angle
if (++m == Ncycles)
{
ch->Position=0; // set current position to Zero
break;
}
if (m==2) dk = -dk;
ignore=300;
}
}
Write3PH_DACs(ch,0,0); // turn off the coil
printf("\nREPORT\n------\n");
for (i=0; i<Ncycles; i++)
printf("%d Position = %6.0f PhaseAngle = %f\n",i,p0[i],kpos[i]/10000.0);
printf("Counts per rev = %6.0f\n",p0[1]-p0[0]);
cnts_per_cycle = (p0[1]-p0[0])/(kpos[1]-kpos[0])*10000.0;
printf("Counts per cycle = %6.0f\n",cnts_per_cycle);
// round to 16
if (cnts_per_cycle>0)
cnts_per_cycle = ((int)(cnts_per_cycle/16.0 + 0.5))*16.0;
else
cnts_per_cycle = ((int)(cnts_per_cycle/16.0 - 0.5))*16.0;
printf("Counts per cycle (rounded to 16)= %6.0f\n",cnts_per_cycle);
ch->invDistPerCycle = 1.0/cnts_per_cycle;
printf("invDistPerCycle (rounded)= %15.12f\n",ch->invDistPerCycle);
mid = (kpos[2]+kpos[1])/20000.0;
mid = mid - (int)mid;
ch->CommutationOffset = mid*cnts_per_cycle + 0.25*fast_fabs(cnts_per_cycle);
printf("Commutation offset = %6.0f\n",ch->CommutationOffset);
printf("Input Gain Specified = %6.3f\n",ch->InputGain0);
}
void main()
{
float k=0,A=10.0f; // set coil current amplitude PWM units
double p0;
WriteSnapAmp(SNAP0+SNAP_SUPPLY_CLAMP0 ,SNAP_CONVERT_VOLTS_TO_ADC(80.0));
WriteSnapAmp(SNAP0+SNAP_SUPPLY_CLAMP_ENA0,1);
WriteSnapAmp(SNAP0+SNAP_PEAK_CUR_LIMIT0,9); // current limit
WriteSnapAmp(SNAP0+SNAP_PEAK_CUR_LIMIT1,9); // current limit
Delay_sec(1); // wait for any fault to clear
// rotate until we find the index mark
ch4->Enable=FALSE;
ch4->OutputChan0=8;
for (;;)
{
Delay_sec(0.001); // wait a millisecond
Write3PH(ch4,A, ++k/1000.0); // move the pole
if (ReadBit(68)) // check for index mark
{
p0=ch4->Position; // save position
ch4->Position=0; // set current position to Zero
// set commutation offset to 1/4th of a cycle
// encoder has 4000 counts/rev
// motor has 3 cycles per rev
ch4->CommutationOffset = 3*8000/2.0/12.0 * 1.02;
printf("Position = %f\n",p0);
break;
}
}
Write3PH(ch4,0,0); // turn off the coil
// define the axis as 3 phase BRUSHLESS_3PH_MODE
// and set low PID gains
ch4->InputMode=ENCODER_MODE;
ch4->OutputMode=BRUSHLESS_3PH_MODE;
ch4->Vel=100000.000000;
ch4->Accel=100000.000000;
ch4->Jerk=100000000.000000;
ch4->P=0.100000;
ch4->I=0.000000;
ch4->D=0.000000;
ch4->FFAccel=0.000000;
ch4->FFVel=0.000000;
ch4->MaxI=200.000000;
ch4->MaxErr=1000000.000000;
ch4->MaxOutput=230.000000;
ch4->DeadBandGain=1.000000;
ch4->DeadBandRange=0.000000;
ch4->InputChan0=4;
ch4->InputChan1=4;
ch4->OutputChan0=8;
ch4->OutputChan1=9;
ch4->LimitSwitchOptions=0x0;
ch4->InputGain0=1.000000;
ch4->InputGain1=1.000000;
ch4->InputOffset0=0.000000;
ch4->InputOffset1=0.000000;
ch4->invDistPerCycle=2.0/8000.0; // CW (2 cycles/rev) / (8000 encoder cnts/rev)
ch4->Lead=0.000000;
ch4->MaxFollowingError=4000.000000;
ch4->StepperAmplitude=20.000000;
ch4->iir[0].B0=1.000000;
ch4->iir[0].B1=0.000000;
ch4->iir[0].B2=0.000000;
ch4->iir[0].A1=0.000000;
ch4->iir[0].A2=0.000000;
ch4->iir[1].B0=1.000000;
ch4->iir[1].B1=0.000000;
ch4->iir[1].B2=0.000000;
ch4->iir[1].A1=0.000000;
ch4->iir[1].A2=0.000000;
ch4->iir[2].B0=1.000000;
ch4->iir[2].B1=0.000000;
ch4->iir[2].B2=0.000000;
ch4->iir[2].A1=0.000000;
ch4->iir[2].A2=0.000000;
EnableAxisDest(4,0.0f); // Enable servo at destination of 0
DefineCoordSystem(4,-1,-1,-1);
}
void main()
{
float mid,A=AMPLITUDE; // set coil current amplitude
int k=0,i,dk=1,WhichSnap,WhichClamp,WhichClampEnable,ignore=300,kpos[Ncycles],zmark,m=0;
double cnts_per_cycle,p0[Ncycles];
CHAN *ch = &chan[AXIS_CHAN];
if (PWM_CHAN < 12)
WhichSnap=SNAP0;
else
WhichSnap=SNAP1;
if (PWM_CHAN==8 || PWM_CHAN==12)
{
WhichClamp=SNAP_SUPPLY_CLAMP0;
WhichClampEnable=SNAP_SUPPLY_CLAMP_ENA0;
}
else
{
WhichClamp=SNAP_SUPPLY_CLAMP1;
WhichClampEnable=SNAP_SUPPLY_CLAMP_ENA1;
}
WriteSnapAmp(WhichSnap+WhichClamp ,SNAP_CONVERT_VOLTS_TO_ADC(CLAMP_VOLTAGE));
WriteSnapAmp(WhichSnap+WhichClampEnable,1);
WriteSnapAmp(WhichSnap+SNAP_PEAK_CUR_LIMIT0,10); // current limit
WriteSnapAmp(WhichSnap+SNAP_PEAK_CUR_LIMIT1,10); // current limit
Delay_sec(1); // wait for any fault to clear
// rotate until we find the index mark
ch->Enable=FALSE;
ch->OutputChan0=PWM_CHAN;
ch->InputMode=ENCODER_MODE;
ch->InputChan0=ENCODER_CHAN;
ch->OutputMode=BRUSHLESS_3PH_MODE;
ch->InputGain0=ENCODER_GAIN;
for (;;)
{
Delay_sec(0.001); // wait a millisecond
k+= dk;
Write3PH(ch,A, k/1000.0); // move the pole
zmark = ReadBit(Z_BIT_NUMBER);
if (!zmark && ignore>0) ignore--;
if (ignore==0 && zmark) // check for index mark
{
p0[m]=ch->Position; // save position
kpos[m]=k; // save phase angle
if (++m == Ncycles)
{
ch->Position=0; // set current position to Zero
break;
}
if (m==2) dk = -dk;
ignore=300;
}
}
Write3PH(ch,0,0); // turn off the coil
// ch4->CommutationOffset = 3*8000/2.0/12.0 * 1.02;
printf("\nREPORT\n------\n");
for (i=0; i<Ncycles; i++)
printf("%d Position = %6.0f PhaseAngle = %f\n",i,p0[i],kpos[i]/1000.0);
printf("Counts per rev = %6.0f\n",p0[1]-p0[0]);
cnts_per_cycle = (p0[1]-p0[0])/(kpos[1]-kpos[0])*1000.0;
printf("Counts per cycle = %6.0f\n",cnts_per_cycle);
// round to 10
if (cnts_per_cycle>0)
cnts_per_cycle = ((int)(cnts_per_cycle/10.0 + 0.5))*10.0;
else
cnts_per_cycle = ((int)(cnts_per_cycle/10.0 - 0.5))*10.0;
printf("Counts per cycle (rounded)= %6.0f\n",cnts_per_cycle);
ch->invDistPerCycle = 1.0/cnts_per_cycle;
printf("invDistPerCycle (rounded)= %15.12f\n",ch->invDistPerCycle);
mid = (kpos[2]+kpos[1])/2000.0;
mid = mid - (int)mid;
ch->CommutationOffset = mid*cnts_per_cycle + 0.25*fast_fabs(cnts_per_cycle);
printf("Commutation offset = %6.0f\n",ch->CommutationOffset);
printf("Input Gain Specified = %6.3f\n",ch->InputGain0);
}
/**
* @par Implementation notes:
*/
bool
W1::_Searcher(uint8_t command, Address &address, Token &token)
{
// Note: 1-based bit counting
uint8_t last_zero_path = 0; // Last zero taken at a discrepancy
uint8_t current_bit = 0; // Current bit counter
uint8_t search_dir = 0; // 1 or 0 for next bit?
uint8_t bits;
// Did we finish last time? Reset state for next call.
if(token == 0xFF) {
token = 0;
return false;
}
// Always reset when beginning a new search
if(!Reset()) {
return false;
}
// Begin search
WriteByte(command);
// Scan down 64 bits of the ROM...
while(++current_bit <= 64) {
bits = ReadBit(); // Next bit value
bits |= ReadBit() << 1; // Complement
switch(bits) {
case 0: // Discrepancy
if(current_bit == token) {
// We took the 0 path, now take the 1 path
search_dir = 1;
}else if(current_bit > token) {
// New discrepancy... take the 0 path
search_dir = 0;
last_zero_path = current_bit;
}else{
// Old discrepancy... take the old path from the address
if(GetBit(address, current_bit-1) == 0) {
search_dir = 0;
last_zero_path = current_bit;
}else{
search_dir = 1;
}
}
break;
case 1: // Only a 1
search_dir = 1;
break;
case 2: // Only a 0
search_dir = 0;
break;
case 3: // No devices!
// Fallthrough
default:
return false;
}
SetBit(address, current_bit-1, search_dir);
WriteBit(search_dir);
}
token = last_zero_path;
// Check if we're done (no more 0 paths for which we still need to search the 1's path)
if(token == 0) {
token = 0xFF;
}
return true;
}
int LuaPacket::Dump(lua_State* L)
{
// Save old rpos and seek to the beginning (without the opcode)
size_t oldrpos = rpos();
rpos(sizeof(uint32_t));
uint32_t pushed = 0;
std::string value = lua_tostring(L, 1);
std::vector<std::string> parts = Utils::Split(value, ' ');
if (parts.size() < 1)
return luaL_argerror(L, 1, "Invalid dump format");
for (uint32_t i = 0; i < parts.size(); i++)
{
if (parts[i] == "int8")
{
ReadInt8(L);
pushed++;
}
else if (parts[i] == "int16")
{
ReadInt16(L);
pushed++;
}
else if (parts[i] == "int32")
{
ReadInt32(L);
pushed++;
}
else if (parts[i] == "int64")
{
ReadInt64(L);
pushed++;
}
else if (parts[i] == "int128")
{
ReadInt128(L);
pushed++;
}
else if (parts[i] == "float")
{
ReadFloat(L);
pushed++;
}
else if (parts[i] == "double")
{
ReadDouble(L);
pushed++;
}
else if (parts[i] == "string")
{
ReadString(L);
pushed++;
}
else if (parts[i] == "bit")
{
ReadBit(L);
pushed++;
}
}
// Restore rpos
rpos(oldrpos);
return pushed;
}
