int main ( void )
{
// Initialize the processor and peripherals.
if ( InitializeSystem() != TRUE )
{
UART2PrintString( "\r\n\r\nCould not initialize USB Custom Demo App - system. Halting.\r\n\r\n" );
while (1);
}
if ( USBHostInit(0) == TRUE )
{
UART2PrintString( "\r\n\r\n***** USB Custom Demo App Initialized *****\r\n\r\n" );
}
else
{
UART2PrintString( "\r\n\r\nCould not initialize USB Custom Demo App - USB. Halting.\r\n\r\n" );
while (1);
}
// Main Processing Loop
while (1)
{
// This demo does not check for overcurrent conditions. See the
// USB Host - Data Logger for an example of overcurrent detection
// with the PIC24F and the USB PICtail Plus.
// Maintain USB Host State
USBHostTasks();
// Maintain Demo Application State
ManageDemoState();
}
return 0;
} // main
// USBTask
static int usbhost_process(struct data_source *ds)
{
// Maintain USB Host State
USBHostTasks();
USBHostUSBBTTask();
return 0;
}
BOOL ConnectionTasks() {
USBHostTasks();
#ifndef USB_ENABLE_TRANSFER_EVENT
USBHostAndroidTasks();
#endif
ConnBTTasks();
ConnADBTasks();
ConnAccessoryTasks();
return USBHostDeviceStatus(0 /* ignored */) == USB_DEVICE_ATTACHED;
}
int main ( void )
{
// Initialize the processor and peripherals.
if ( InitializeSystem() != TRUE )
{
UART2PrintString( "\r\n\r\nCould not initialize USB Custom Demo App - system. Halting.\r\n\r\n" );
while (1);
}
if ( USBHostInit(0) == TRUE )
{
UART2PrintString( "\r\n\r\n***** USB Custom Demo App Initialized *****\r\n\r\n" );
}
else
{
UART2PrintString( "\r\n\r\nCould not initialize USB Custom Demo App - USB. Halting.\r\n\r\n" );
while (1);
}
btClientData.State = BT_STATE_IDLE;
btClientData.Initialized = FALSE;
mPORTAOutputConfig(0x3);
mPORTAWrite(0x0);
mPORTBOutputConfig(0x10);
// OC 1
PPSOutput(PPS_RP4, PPS_OC1);
//Enable Interrupt
SetPriorityIntOC1(4);
EnableIntOC1;
OpenTimer2(T2_ON | T2_PS_1_8 ,0xffff); //
OpenOC1(OC_IDLE_CON | OC_TIMER2_SRC | OC_PWM_EDGE_ALIGN ,OC_SYNC_TRIG_IN_TMR2,0,0);
SetDCOC1PWM(0xc00,0);
// Main Processing Loop
while (1)
{
BTClientTasks();
// Maintain USB Host State
USBHostTasks();
DelayMs(1);
}
return 0;
} // main
/*******************************************************************************
Function:
bool USBHostCDC_ApiDeviceDetect( void )
Description:
This function determines if a CDC device is attached
and ready to use.
Precondition:
None
Parameters:
None
Return Values:
true - CDC present and ready
false - CDC not present or not ready
Remarks:
Since this will often be called in a loop while waiting for
a device, we'll make sure the tasks are executed.
*******************************************************************************/
bool USBHostCDC_ApiDeviceDetect( void )
{
USBHostTasks();
USBHostCDCTasks();
if ((USBHostCDCDeviceStatus(CDCdeviceAddress) == USB_CDC_NORMAL_RUNNING) &&
(CDCdeviceAddress != 0))
{
return true;
}
return false;
}
/*******************************************************************************
Function:
BOOL USBHostCDC_ApiDeviceDetect( void )
Description:
This function determines if a CDC device is attached
and ready to use.
Precondition:
None
Parameters:
None
Return Values:
TRUE - CDC present and ready
FALSE - CDC not present or not ready
Remarks:
Since this will often be called in a loop while waiting for
a device, we'll make sure the tasks are executed.
*******************************************************************************/
BOOL USBHostCDC_ApiDeviceDetect( void )
{
USBHostTasks();
USBHostCDCTasks();
if ((USBHostCDCDeviceStatus(CDCdeviceAddress) == USB_CDC_NORMAL_RUNNING) &&
(CDCdeviceAddress != 0))
{
return TRUE;
}
return FALSE;
}
/**
* Execute run_loop
*/
void embedded_execute(void) {
data_source_t *ds;
while (1) {
USBHostTasks();
// process data sources
data_source_t *next;
for (ds = (data_source_t *) data_sources; ds != NULL ; ds = next){
next = (data_source_t *) ds->item.next; // cache pointer to next data_source to allow data source to remove itself
ds->process(ds);
}
#ifdef HAVE_TICK
// process timers
while (timers) {
timer_source_t *ts = (timer_source_t *) timers;
if (ts->timeout > system_ticks) break;
run_loop_remove_timer(ts);
ts->process(ts);
}
#endif
#if USE_SPP_SERVER || USE_DUALSHOCK3_RUMBLE
sendchar();
#endif
// disable IRQs and check if run loop iteration has been requested. if not, go to sleep
#if 0
hal_cpu_disable_irqs();
if (trigger_event_received){
hal_cpu_enable_irqs_and_sleep();
continue;
}
hal_cpu_enable_irqs();
#else
Idle();
#endif
#ifndef __DEBUG
if(!deviceAddress){
Reset();
}
#endif
}
}
/********************************************************************
* Function: void main(void)
*
* PreCondition: None
*
* Input: None
*
* Output: None
*
* Side Effects: None
*
* Overview: Main program entry point.
*
* Note: None
*******************************************************************/
MAIN_RETURN main(void)
{
SYSTEM_Initialize(SYSTEM_STATE_USB_HOST);
//Initialize the stack
USBHostInit(0);
APP_HostHIDMouseInitialize();
while(1)
{
USBHostTasks();
USBHostHIDTasks();
//Application specific tasks
APP_HostHIDMouseTasks();
}//end while
}//end main
/********************************************************************
* Function: void main(void)
*
* PreCondition: None
*
* Input: None
*
* Output: None
*
* Side Effects: None
*
* Overview: Main program entry point.
*
* Note: None
*******************************************************************/
int main(void)
{
SYS_Initialize();
FILEIO_Initialize();
FILEIO_RegisterTimestampGet(GetTimestamp);
//Initialize the stack
USBHostInit(0);
APP_HostMSDSimpleInitialize();
while(1)
{
USBHostTasks();
USBHostMSDTasks();
//Application specific tasks
APP_HostMSDSimpleTasks();
}//end while
}//end main
int main ( void )
{
if ( InitializeSystem() != TRUE )
{
UART2PrintString( "\r\n\r\nCould not initialize USB Custom Demo App - system. Halting.\r\n\r\n" );
while (1);
}
if ( USBHostInit(0) == TRUE )
{
UART2PrintString( "\r\n\r\n***** USB Custom Demo App Initialized *****\r\n\r\n" );
}
else
{
UART2PrintString( "\r\n\r\nCould not initialize USB Custom Demo App - USB. Halting.\r\n\r\n" );
while (1);
}
while (1)
{
USBHostTasks();
ManageDemoState();
}
return 0;
} // main
void dev_elecom_u3412s(void)
{
log_info( "\r\n ELECOM U3412S device attached\r\n\r\n" );
GENERAL_CONTROLLER_STATE state = CONTROLLER_INITIALIZE;
while(deviceAddress){
unsigned char hid_packet[64];
switch(state){
case CONTROLLER_INITIALIZE:
if (!USBHostBluetoothRxEventIsBusy(deviceAddress)){
state = CONTROLLER_STATE_GET_DATA;
}
break;
case CONTROLLER_STATE_GET_DATA:
if(USBHostBluetoothReadEvent(deviceAddress,hid_packet,64) == USB_SUCCESS){
state = CONTROLLER_STATE_GET_DATA_WAIT;
}
break;
case CONTROLLER_STATE_GET_DATA_WAIT:
if (!USBHostBluetoothRxEventIsBusy(deviceAddress)){
state = CONTROLLER_STATE_GET_DATA;
#if 0
int i;
for(i = 0;i < 26;i++){
log_info("%2x",hid_packet[i]);
}
log_info( "GET DATA\r\n" );
#endif
elecom_u3412s_process_packet2(hid_packet,64);
out_button_state();
}
break;
case CONTROLLER_STATE_ERROR:
break;
}
USBHostTasks();
}
}
//******************************************************************************
//******************************************************************************
// Thunks to the underlying USB HOST MAL code
//******************************************************************************
//******************************************************************************
void ChipKITUSBHost::Tasks()
{
USBHostTasks();
}
/********************************************************************
* Function: void main(void)
*
* PreCondition: None
*
* Input: None
*
* Output: None
*
* Side Effects: None
*
* Overview: Main program entry point.
*
* Note: None
*******************************************************************/
void main(void) {
// test command buffer
// if(0)
// {
// UB_Init();
//
// UB_ProcessCommand( STX);
// UB_ProcessCommand( STX);
// UB_ProcessCommand( 0x76);
// UB_ProcessCommand( 0x8A);
// UB_ProcessCommand( ETX);
//
// UB_ProcessCommand( STX);
// UB_ProcessCommand( STX);
// UB_ProcessCommand( 0);
// UB_ProcessCommand( ETX);
//
// APP_HostHIDPICkitTasks();
//
// UB_ProcessCommand( STX);
// UB_ProcessCommand( STX);
// UB_ProcessCommand( 0);
// UB_ProcessCommand( ETX);
// }
//------------- test UART ----------------
setup();
// if(0)
// {
// //test Timer_1ms
// TIMER_SetConfiguration ( TIMER_CONFIGURATION_1MS );
// TIMER_RequestTick( blinkLED, 500); //500ms
//
// //test UART2
// IEC1bits.U2RXIE= 1; //enable RXIE
//
// }
//--------------------------------
SYSTEM_Initialize(SYSTEM_STATE_USB_HOST);
//Initialize the stack
USBHostInit(0);
APP_HostHIDPICkitInitialize();
UB_Init(); // UART_Buffer
while (1) {
USBHostTasks();
USBHostHIDTasks();
//Application specific tasks
APP_HostHIDPICkitTasks();
UB_TxTasks();
LATBbits.LATB3 ^= 1;
}//end while
}//end main
int main(void)
{
int i;
BRC_InitializeLEDSW();
BRC_LEDRed(1);
BRC_LEDGreen(0);
BRC_SetupClockPLL();
BRC_SetupUSBClock();
BRC_InitializeSysClock(10000l);
BRC_IORemap();
BRC_InitializeMotorPulse();
// MotorPulse RP bind
StartIORemap();
RPOUT_CLK1=BRC_OCxRPO_MPCH(0);
RPOUT_CLK2=BRC_OCxRPO_MPCH(1);
RPOUT_CLK3=BRC_OCxRPO_MPCH(2);
//RPOUT_CLK4=BRC_OCxRPO_MPCH(3);
//RPOUT_CLK5=BRC_OCxRPO_MPCH(4);
//RPOUT_CLK6=BRC_OCxRPO_MPCH(5);
//RPOUT_DE21=BRC_OCxRPO_MPCH(1);
//RPOUT_DE22=BRC_OCxRPO_MPCH(2);
EndIORemap();
// Serial Port Binding
#if 0
StartIORemap();
RPINR19bits.U2RXR=RPIN_DE22; // UART 2 RX <- RP_DE22
RPOUT_DE21=3; // RP_DE21 <- UART 2 TX
EndIORemap();
InitializeSerial(SIB115200);
#endif
InitializeFutabaServo(SIB115200);
TRIS_ENABLE12=TRIS_OUT; TRIS_DCY12=TRIS_OUT;
TRIS_M12=TRIS_OUT; TRIS_TQ12=TRIS_OUT;
LAT_ENABLE12=0; LAT_DCY12=0; LAT_M12=0; LAT_TQ12=0;
TRIS_CWCCW1=TRIS_OUT; TRIS_CWCCW2=TRIS_OUT; TRIS_CWCCW3=TRIS_OUT;
LAT_CWCCW1=0; LAT_CWCCW2=0; LAT_CWCCW3=0;
BRC_SetMotorSpeed(0,0);
BRC_SetMotorSpeed(1,0);
BRC_SetMotorSpeed(2,0);
BRC_InitializeLEDSwitchBoard();
InitializeMPU6050();
ReadMPU6050(1);
#define Count RegFileL[0]
#define ControlCount RegFileL[1]
Count=0;
ControlCount=0;
InitializeSerialLoop(1,SLB115200,4,"BRC_USBtest " __DATE__ " " __TIME__);
SLReply32(63,1,0x12345678);
SLReplyProfile(63,"start up");
for(i=0;i<16;i++)
{
RegFileL[i]=0; RegFileS[i]=0;
}
// USB_JOYPAD ; do not change or use carefully
// Initializer for USB interface and JoyPad handler
USBHostInit(0);
APP_HostHIDJoyPadInitialize();
// /USB_JOYPAD
// ANSEL: digital config (0)
// ANSELA=0x0000;
ANSELB=0x0000; ANSELC=0x0000;
ANSELD=0x0000; ANSELE=0x0000;
// ANSELF=0x0000;
ANSELG=0x0000;
//TRIS_CCN10=TRIS_OUT; TRIS_CCN11=TRIS_OUT;
//TRIS_CCN12=TRIS_OUT; TRIS_CCN13=TRIS_OUT;
//TRIS_CCN14=TRIS_OUT; TRIS_CCN15=TRIS_OUT;
//TRIS_CCN16=TRIS_OUT; TRIS_CCN17=TRIS_OUT;
long clockstep=20;
long nextclock=BRC_SysClock+clockstep;
// state variables
enum { SVCM_Position, SVCM_Velocity, SVCM_Lean };
struct _SV
{
long theta, thetav, pos, vel;
long theta_ref, thetav_ref, pos_ref, vel_ref, pos_ref_0;
long theta_0;
long acc;
int cm; // controlmode 0: pos 1: vel 2: agl ; SVCM_*
int gm[4]; // gainmask 1,0,-1
} SV[2];
#define SVX (SV[0])
#define SVY (SV[1])
SVX.pos=0; SVY.pos=0;
SVX.cm=99; SVY.cm=99;
SVX.theta_0=SVY.theta_0=0;
for(i=0;i<4;i++) { SVX.gm[i]=SVY.gm[i]=1; }
int YawRate=0;
RegFileL[4]= 400;
RegFileL[5]=3500;
RegFileL[6]= 500;
RegFileL[7]=5000;
FSServoOnOff(1,1);
while(1)
{
RegFileL[2]=nextclock-BRC_SysClock;
while(nextclock>BRC_SysClock)
{
// USB_JOYPAD ; do not change or use carefully
// These three functions are required for state machine tasks
// of USB handlers; execute while main tasks are idel
USBHostTasks();
USBHostHIDTasks();
APP_HostHIDJoyPadTasks();
// /USB_JOYPAD
}
Count++; nextclock+=clockstep;
// main control
// lean angle sensing
// if zero detect commanded
if((((padState.button&0x300)==0x300)||(BRC_SWState()))&&(!IsIMUZeroDetect()))
{
ReadMPU6050(1); // zero detect mode
SVX.theta_0=SVY.theta_0=0;
}
else
ReadMPU6050(0); // normal IMU function
if(IsIMUZeroDetect())
BRC_LEDGreen(1);
else
BRC_LEDGreen(0);
unsigned long lastcc=ControlCount;
if(((padState.button&0x1)==0x1)&&(!IsIMUZeroDetect()))
{
if(ControlCount<1000)
ControlCount=1000;
}
if((((padState.button&0x5)==0x5)||(BRC_LEDSwitchBoard_GetPush()))&&(!IsIMUZeroDetect()))
{
ControlCount=10000000l;
}
if((padState.button&0xa)==0xa)
{
ControlCount=0;
}
if((lastcc==0)&&(ControlCount>0))
{ // Control started now
LAT_ENABLE12=1;
SVX.theta_ref=0; SVX.pos_ref_0=SVX.pos_ref=SVX.pos;
SVY.theta_ref=0; SVY.pos_ref_0=SVY.pos_ref=SVY.pos;
SVX.vel=0;
SVY.vel=0;
SVX.thetav_ref=0; SVX.vel_ref=0;
SVY.thetav_ref=0; SVY.vel_ref=0;
BRC_LEDRed(1);
}
if(ControlCount>0) ControlCount--;
if((lastcc>0)&&(ControlCount==0))
{ // Control stopped now
LAT_ENABLE12=0;
BRC_LEDRed(0);
}
SVX.theta = imu.CGAngleY - SVX.theta_0; // lean to x axis dir
SVX.thetav= imu.GyroAVY; // angular vel of above
SVY.theta =-imu.CGAngleX - SVY.theta_0; // lean to y axis dir
SVY.thetav=-imu.GyroAVX; // angular vel of above
if(ControlCount>0)
{ // main control
for(i=0;i<2;i++)
{
SV[i].acc=SDR8(
SDR12((long)(RegFileL[4])*(SV[i].gm[0])*(SV[i].theta -SV[i].theta_ref))+
SDR12((long)(RegFileL[5])*(SV[i].gm[1])*(SV[i].thetav-SV[i].thetav_ref))+
SDR8( (long)(RegFileL[6])*(SV[i].gm[2])*(SV[i].pos -SV[i].pos_ref))+
SDR8( (long)(RegFileL[7])*(SV[i].gm[3])*(SV[i].vel -SV[i].vel_ref))
);
SV[i].vel+=SDR4(SV[i].acc);
SV[i].pos+=SDR4(SV[i].vel);
}
if((SVX.vel>30000)||(SVX.vel<-30000)) ControlCount=0; // force stop
if((SVY.vel>30000)||(SVY.vel<-30000)) ControlCount=0;
int ps0=SVX.vel;
int ps1=SVY.vel;
int ps2=YawRate;
int ms0= ps0 + +ps2;
int ms1=-(ps0>>1)+(int)(((long)ps1*222)>>8)+ps2;
int ms2=-(ps0>>1)-(int)(((long)ps1*222)>>8)+ps2;
BRC_SetMotorSpeed(0,ms0);
BRC_SetMotorSpeed(1,ms1);
BRC_SetMotorSpeed(2,ms2);
// lean angle equilibrium point
if((padState.button&0x100)==0x100)
for(i=0;i<2;i++)
{
SV[i].theta_0-=SDR4(SV[i].pos-SV[i].pos_ref);
}
}
else
{
SVX.vel=0;
SVY.vel=0;
BRC_SetMotorSpeed(0,0);
BRC_SetMotorSpeed(1,0);
BRC_SetMotorSpeed(2,0);
LAT_ENABLE12=0;
BRC_LEDRed(0);
}
BRC_LEDSwitchBoard_CycleTask();
// USB_JOYPAD ; do not change or use carefully
// request the JoyPad current stick/button state
// results will be stored into padState (global) structure
// in subsequent tasks in USB handlers
if((Count&0xf)==0)
APP_HostHIDInputRequest();
// /USB_JOYPAD
//BRC_LEDRed((count&1)?1:0);
#if 1
//BRC_LEDSwitchBoard_SetLEDRaw(0x55aa55aal);
//BRC_LEDSwitchBoard_SetHex(0x0123);
//BRC_LEDSwitchBoard_SetHex(BRC_LEDSwitchBoard_GetSwitch());
//BRC_LEDSwitchBoard_SetHex(Count);
//BRC_LEDSwitchBoard_SetHex((int)(imu.CGAngleX>>8)+0x8000);
//BRC_LEDSwitchBoard_SetHex(padState.axis[0]);
//BRC_LEDRed(BRC_SWState());
BRC_LEDRed(BRC_LEDSwitchBoard_GetPush());
//BRC_LEDSwitchBoard_SetLEDRaw(RegFileL[4]);
//BRC_LEDSwitchBoard_SetLED(RegFileL[4]);
BRC_LEDSwitchBoard_SetLEDBits(0,IsIMUZeroDetect());
BRC_LEDSwitchBoard_SetLEDBits(1,(ControlCount>0)?1:0);
// BRC_LEDSwitchBoard_SetHex((int)(imu.CGAngleX>>8));
// BRC_LEDSwitchBoard_SetSIntZS((int)(imu.CGAngleX>>8),3);
#define IntAngleToDeg(a) ((((a)>>12)*358)>>9)
#define IntAngleVelToDegps(a) ((((a)>>10)*625)>>6)
#define IntPosToMm(a) ((((a)>>5)*267)>>8)
#define IntVelToMmps(a) (((a)*209)>>11)
switch((BRC_LEDSwitchBoard_GetSwitch()>>12)&0xf)
{
case 0: BRC_LEDSwitchBoard_SetSIntZS((int)IntAngleToDeg(SVX.theta),3); BRC_LEDSwitchBoard_SetDP(2); break;
case 1: BRC_LEDSwitchBoard_SetSIntZS((int)IntAngleToDeg(SVY.theta),3); BRC_LEDSwitchBoard_SetDP(2); break;
case 2: BRC_LEDSwitchBoard_SetSIntZS((int)IntAngleVelToDegps(SVX.thetav),2); BRC_LEDSwitchBoard_SetDP(1); break;
case 3: BRC_LEDSwitchBoard_SetSIntZS((int)IntAngleVelToDegps(SVY.thetav),2); BRC_LEDSwitchBoard_SetDP(1); break;
case 4: BRC_LEDSwitchBoard_SetSIntZS((int)IntPosToMm(SVX.pos),2); BRC_LEDSwitchBoard_SetDP(1); break;
case 5: BRC_LEDSwitchBoard_SetSIntZS((int)IntPosToMm(SVY.pos),2); BRC_LEDSwitchBoard_SetDP(1); break;
case 6: BRC_LEDSwitchBoard_SetSIntZS((int)IntVelToMmps(SVX.vel),1); BRC_LEDSwitchBoard_SetDP(0); break;
case 7: BRC_LEDSwitchBoard_SetSIntZS((int)IntVelToMmps(SVY.vel),1); BRC_LEDSwitchBoard_SetDP(0); break;
case 8: BRC_LEDSwitchBoard_SetUInt(Count); break;
case 9: BRC_LEDSwitchBoard_SetSInt(RegFileL[2]&0xffff); break;
case 10: BRC_LEDSwitchBoard_SetSIntZS((int)IntAngleToDeg(imu.CGAngleX),3); BRC_LEDSwitchBoard_SetDP(2); break;
case 11: BRC_LEDSwitchBoard_SetSIntZS((int)IntAngleToDeg(imu.CGAngleY),3); BRC_LEDSwitchBoard_SetDP(2); break;
case 12: BRC_LEDSwitchBoard_SetSIntZS((int)IntAngleVelToDegps(imu.GyroAVX),2); BRC_LEDSwitchBoard_SetDP(1); break;
case 13: BRC_LEDSwitchBoard_SetSIntZS((int)IntAngleVelToDegps(imu.GyroAVY),2); BRC_LEDSwitchBoard_SetDP(1); break;
}
#endif
for(i=0;i<2;i++)
{
switch(SV[i].cm)
{
case SVCM_Position: BRC_LEDSwitchBoard_SetLEDBits(2+i,0); break;
case SVCM_Velocity: BRC_LEDSwitchBoard_SetLEDBits(2+i,1); break;
case SVCM_Lean: BRC_LEDSwitchBoard_SetLEDBits(2+i,(Count&0x10)?1:0); break;
}
}
if(padState.updated)
{
int ps[3];
int sh=2;
ps[0]=DeadZone((int)((padState.axis[0])-0x8000)>>1,0x500);
ps[1]=DeadZone((int)((padState.axis[1])-0x8000)>>1,0x500);
ps[2]=DeadZone((int)((padState.axis[2])-0x8000)>>1,0x500);
padState.updated=0;
//SLReply16(63,0,padState.axis[0]);
//SLReply16(63,1,(int)((((int)(padState.axis[0]))-0x8000))>>5);
//SLReply16(63,4,padState.button);
if(padState.button&0x0008) sh=0;
YawRate=ps[2]>>(3+sh);
// controlmode transition
for(i=0;i<2;i++)
{
int ocm=SV[i].cm;
SV[i].cm=SVCM_Position;
if(padState.button&(0x010<<(i*2))) SV[i].cm=SVCM_Velocity;
if(padState.button&(0x020<<(i*2))) SV[i].cm=SVCM_Lean;
if(SV[i].cm!=ocm) // mode_changed
{
switch(SV[i].cm)
{
case SVCM_Position:
SV[i].gm[0]=1; SV[i].gm[1]=1; SV[i].gm[2]=1; SV[i].gm[3]=1; // gain activation
SV[i].pos_ref_0=SV[i].pos_ref=SV[i].pos;
SV[i].vel_ref=0; SV[i].theta_ref=0;
break;
case SVCM_Velocity:
SV[i].gm[0]=1; SV[i].gm[1]=1; SV[i].gm[2]=0; SV[i].gm[3]=1; // gain activation
SV[i].vel_ref=0; SV[i].theta_ref=0;
break;
case SVCM_Lean:
SV[i].gm[0]=1; SV[i].gm[1]=1; SV[i].gm[2]=0; SV[i].gm[3]=0; // gain activation
SV[i].theta_ref=0;
break;
}
}
switch(SV[i].cm)
{
case SVCM_Position:
SV[i].pos_ref=SV[i].pos_ref_0+((long)(ps[i])<<(4-sh));
break;
case SVCM_Velocity:
SV[i].vel_ref=ps[i]>>(sh);
break;
case SVCM_Lean:
SV[i].theta_ref=(long)(ps[i])<<(8-sh);
break;
}
}
}