void main(void)
{
WDTCTL = WDTPW+WDTHOLD; // Paramos el watchdog timer
init_botons(); // Iniciamos los botones y Leds.
_enable_interrupt(); // Activamos las interrupciones a nivel global del chip
init_LCD(); // Inicializamos la pantalla
init_UCS(); //Inicialitzem UCS
init_UART(); //Inicialitzem UART
halLcdPrintLine( saludo,linea,textstyle);
linea++;
sprintf(cadena,"bID = %d",bID);
halLcdPrintLine( cadena,linea,textstyle);
Encendre_LED();
TxPacket(0xFE, 2, 0x19);
do
{
P1OUT ^= 0x03;
i = 25000;
do {
i--;
}
while (i != 0);
}
while(1);
}
// read from ALL ax
void readPositionData(byte* buffer, byte &failure)
{
int id, i, len;
// command: read AX12_DATA_READ bytes from address P_PRESENT_POSITION_L /ax12
gbpParameter[0] = P_PRESENT_POSITION_L; //Address
gbpParameter[1] = AX12_DATA_READ; //Read Length
// send command to every ax12 and store result in readBuffer[]
// failure = 1 signals a read failure
failure = AX12_NOERROR;
for(id=0; id<AX12_COUNT; id++)
{
failure = AX12_ERROR;
i = 0;
// on failure: retry AX12_RETRY times
while ( (failure == AX12_ERROR) && (i < AX12_RETRY))
{
TxPacket(id + AX12_STARTID,INST_READ,2);
if(RxPacket(DEFAULT_RETURN_PACKET_SIZE+gbpParameter[1]) == DEFAULT_RETURN_PACKET_SIZE+gbpParameter[1])
{
for (len=0; len<AX12_DATA_READ; len++)
buffer[id*AX12_DATA_READ + len] = gbpRxBuffer[5 + len];
failure = AX12_NOERROR;
} else i++;
}
if (failure == AX12_ERROR)
break;
}
}
/**
* @param none
*/
void nRF905_txData(void)
{
/* Set nRF905 in Tx mode */
set_tx();
nrf_delay_ms(10);
TxPacket();
nrf_delay_ms(10);
}
// ptp movement (sinus wave acceleration)
void writePositionDataPTPSinus(unsigned long readPause, byte* readBuffer,byte* writeBuffer, float pos, float* tb, float* te, int* sgn, float* vmax, float* bmax)
{
int i;
long int current;
float se, t, tv;
// sync write AX12_DATA_WRITE bytes starting at address P_GOAL_POSITION_L
byte op = INST_SYNC_WRITE;
gbpParameter[0] = P_GOAL_POSITION_L; //Address
gbpParameter[1] = AX12_DATA_WRITE;
// prepare write buffer -> unpack data from serial and store bytewise in buffer
for (i=0;i<AX12_COUNT;i++)
{
gbpParameter[2 + i * (AX12_DATA_WRITE+1)] = i + 1;
current = readBuffer[i * AX12_DATA_WRITE + 0] + 256 * readBuffer[i * AX12_DATA_WRITE + 1];
t = pos * te[i];
tv = te[i] - tb[i];
if (t <= tb[i])
se = bmax[i] * (0.25 * t * t + (tb[i] * tb[i] / (8.0 * M_PI * M_PI)) * (cos(2.0*M_PI*t/tb[i])-1.0));
else if (t <= te[i] - tb[i])
se = vmax[i] * (t - 0.5 * tb[i]);
else
se = 0.5 * bmax[i] * (te[i] * (t + tb[i]) - 0.5*(t*t + te[i]*te[i] + 2.0 * tb[i]*tb[i]) + (tb[i]*tb[i]/(4.0 * M_PI * M_PI))*(1.0 - cos((2.0*M_PI/tb[i])*(t - tv))));
current += sgn[i] * (long int) (se);
if (current > 1023)
current = 1023;
else if (current < 0)
current = 0;
gbpParameter[3 + i * (AX12_DATA_WRITE+1) + 0] = current & 0xFF;
gbpParameter[3 + i * (AX12_DATA_WRITE+1) + 1] = (current & 0xFF00) >> 8;
/*
if (i == 13)
{
uart1_putc(current & 0xFF);
uart1_putc((current & 0xFF00) >> 8);
}
*/
// store target speed and torque in write buffer
gbpParameter[3 + i * (AX12_DATA_WRITE+1) + 2] = writeBuffer[i * AX12_DATA_WRITE + 2];
gbpParameter[3 + i * (AX12_DATA_WRITE+1) + 3] = writeBuffer[i * AX12_DATA_WRITE + 3];
gbpParameter[3 + i * (AX12_DATA_WRITE+1) + 4] = writeBuffer[i * AX12_DATA_WRITE + 4];
gbpParameter[3 + i * (AX12_DATA_WRITE+1) + 5] = writeBuffer[i * AX12_DATA_WRITE + 5];
}
// send all data to ax12s AT ONCE
TxPacket(BROADCASTING_ID,op,2+AX12_COUNT*(AX12_DATA_WRITE + 1));
}
void NRF905::TX(char *TxRxBuf, char *TxAddress)
{
set_tx();
delay(1);
// Send data by nRF905
TxPacket(TxAddress, TxRxBuf);
set_rx();
}
void NRF905::TX(char *TxRxBuf)
{
set_tx();
delay(1);
// Send data by nRF905
TxPacket(config_info_buf+5, TxRxBuf);
set_rx(); //switch back to receiving mode to set DR low
}
void AX_Action(U08 ID, U08 Position_H, U08 Position_L, U08 Speed_H, U08 Speed_L)
//ID가 일치하는 모터가 position으로 setting된 speed로 이동한다.
{
Parameter[0] = P_GOAL_POSITION_L; //goal position address : 30
Parameter[1] = Position_L; //Low 8bit at goal position
Parameter[2] = Position_H; //High 8bit at goal position
Parameter[3] = Speed_L; //Low 8bit at moving speed
Parameter[4] = Speed_H; //High 8bit at moving speed
TxPacketLength = TxPacket(ID,INST_WRITE,5);
RxPacketLength = RxPacket(DEFAULT_RETURN_PACKET_SIZE);
}
BOOL PXA271_USB_Driver::StartOutput( USB_CONTROLLER_STATE* State, int endpoint )
{
ASSERT( State );
ASSERT(endpoint < c_Used_Endpoints);
GLOBAL_LOCK(irq);
// If endpoint is not an output
if( State->Queues[endpoint] == NULL || !State->IsTxQueue[endpoint] )
return FALSE;
/* if the halt feature for this endpoint is set, then just
clear all the characters */
if(State->EndpointStatus[endpoint] & USB_STATUS_ENDPOINT_HALT)
{
ClearTxQueue( State, endpoint );
return TRUE;
}
//If TxRunning, interrupts will drain the queue
if(!g_PXA271_USB_Driver.TxRunning[endpoint])
{
g_PXA271_USB_Driver.TxRunning[endpoint] = TRUE;
// Calling both TxPacket & EP_TxISR in this routine could cause a TX FIFO overflow
TxPacket( State, endpoint );
}
else if(irq.WasDisabled())
{
PXA271_USB& USB = PXA271::USB();
// This could be called during Flush with all interrupts off. Just taking care of the endpoint
// in question may cause a logjam if the host is expecting a response from another endpoint.
// All endpoints must be examined for activity.
if( USB.UDCISR0 & USB.UDCICR0 & USB.UDCICR__BOTH ) // If endpoint 0 needs attention
EP0_ISR( 0 );
for( int ep = 1; ep < c_Used_Endpoints; ep++ )
{
// If no interrupt for this endpoint
if( (((USB.UDCISR0 & USB.UDCICR0) >> (ep * 2)) & USB.UDCICR__BOTH) == 0 )
continue;
if(State->Queues[ep])
{
if( State->IsTxQueue[endpoint] )
EP_TxISR( endpoint );
else
EP_RxISR( endpoint );
}
}
}
// ptp movement (constant acceleration)
void writePositionDataPTP(DynamixelComm *dc, unsigned long readPause, byte* readBuffer,byte* writeBuffer, float pos, float* tb, float* te, int* sgn, float* vmax, float* bmax)
{
int i;
long int current;
float se, t;
// sync write AX12_DATA_WRITE bytes starting at address P_GOAL_POSITION_L
byte op = INST_SYNC_WRITE;
gbpParameter[0] = P_GOAL_POSITION_L; //Address
gbpParameter[1] = AX12_DATA_WRITE;
printf("%s joints: ", __func__);
// prepare write buffer -> unpack data from serial and store bytewise in buffer
for (i=0;i<AX12_COUNT;i++)
{
gbpParameter[2 + i * (AX12_DATA_WRITE+1)] = i + 1;
current = readBuffer[i * AX12_DATA_WRITE + 0] + 256 * readBuffer[i * AX12_DATA_WRITE + 1];
t = pos * te[i];
if (t <= tb[i])
se = 0.5 * bmax[i] * t * t;
else if (t <= te[i] - tb[i])
se = (vmax[i] * t - 0.5 * vmax[i] * vmax[i] / bmax[i]);
else
se = (vmax[i] * (te[i] - tb[i]) - 0.5 * bmax[i] * (te[i] - t) * (te[i] - t));
current += sgn[i] * (long int) (se);
if (current > 1023)
current = 1023;
else if (current < 0)
current = 0;
gbpParameter[3 + i * (AX12_DATA_WRITE+1) + 0] = current & 0xFF;
gbpParameter[3 + i * (AX12_DATA_WRITE+1) + 1] = (current & 0xFF00) >> 8;
printf("%04lx ", current);
// store target speed and torque in write buffer
gbpParameter[3 + i * (AX12_DATA_WRITE+1) + 2] = writeBuffer[i * AX12_DATA_WRITE + 2];
gbpParameter[3 + i * (AX12_DATA_WRITE+1) + 3] = writeBuffer[i * AX12_DATA_WRITE + 3];
gbpParameter[3 + i * (AX12_DATA_WRITE+1) + 4] = writeBuffer[i * AX12_DATA_WRITE + 4];
gbpParameter[3 + i * (AX12_DATA_WRITE+1) + 5] = writeBuffer[i * AX12_DATA_WRITE + 5];
}
printf("\n");
// send all data to ax12s AT ONCE
TxPacket(dc, BROADCASTING_ID,op,2+AX12_COUNT*(AX12_DATA_WRITE + 1));
}
// write position data to all ax
// simple linear interpolation (NOT very smooth)
void writePositionData(DynamixelComm *dc, byte* readBuffer, byte* writeBuffer, long int pos, long int max)
{
// sync write AX12_DATA_WRITE bytes starting at address P_GOAL_POSITION_L
byte op = INST_SYNC_WRITE;
gbpParameter[0] = P_GOAL_POSITION_L; //Address
gbpParameter[1] = AX12_DATA_WRITE;
// conversion variables
int i;
long int current, target;
long int tmp;
double cp = (double)pos / (double)max;
printf("%s joints: ", __func__);
// prepare write buffer -> unpack data from serial and store bytewise in buffer
for (i=0;i<AX12_COUNT;i++)
{
gbpParameter[2 + i * (AX12_DATA_WRITE+1)] = i + AX12_STARTID;
// simple linear interpolation
target = writeBuffer[i * AX12_DATA_WRITE + 0] + 256 * writeBuffer[i * AX12_DATA_WRITE + 1];
current = readBuffer[i * AX12_DATA_WRITE + 0] + 256 * readBuffer[i * AX12_DATA_WRITE + 1];
tmp = target - current;
double tmp2 = (double)tmp * cp;
current += (long int)tmp2;
if (current > 1023)
current = 1023;
else if (current < 0)
current = 0;
gbpParameter[3 + i * (AX12_DATA_WRITE+1) + 0] = current & 0xFF;
gbpParameter[3 + i * (AX12_DATA_WRITE+1) + 1] = (current & 0xFF00) >> 8;
printf("%04lx ", current);
// store target speed and torque in write buffer
gbpParameter[3 + i * (AX12_DATA_WRITE+1) + 2] = writeBuffer[i * AX12_DATA_WRITE + 2];
gbpParameter[3 + i * (AX12_DATA_WRITE+1) + 3] = writeBuffer[i * AX12_DATA_WRITE + 3];
gbpParameter[3 + i * (AX12_DATA_WRITE+1) + 4] = writeBuffer[i * AX12_DATA_WRITE + 4];
gbpParameter[3 + i * (AX12_DATA_WRITE+1) + 5] = writeBuffer[i * AX12_DATA_WRITE + 5];
}
printf("\n");
// send all data to ax12s AT ONCE
TxPacket(dc, BROADCASTING_ID,op,2+AX12_COUNT*(AX12_DATA_WRITE + 1));
}
void TxPacketBytes(char header, char data0, char data1, char data2, char data3)
{
char buffer[8];
char checksum;
checksum = buffer[0] = STARTBYTE1;
checksum += buffer[1] = STARTBYTE2;
checksum += buffer[2] = header;
checksum += buffer[3] = data0;
checksum += buffer[4] = data1;
checksum += buffer[5] = data2;
checksum += buffer[6] = data3;
buffer[7] = checksum;
TxPacket(buffer);
}
void TxString(char header, char payload[])
{
char buffer[PACKETLENGTH];
char checksum;
checksum = buffer[0] = STARTBYTE1;
checksum += buffer[1] = STARTBYTE2;
checksum += buffer[2] = header;
checksum += buffer[3] = payload[0];
checksum += buffer[4] = payload[1];
checksum += buffer[5] = payload[2];
checksum += buffer[6] = payload[3];
buffer[7] = checksum;
TxPacket(buffer);
}
BOOL AT91_USBHS_Driver::StartOutput( USB_CONTROLLER_STATE* State, int endpoint )
{
ASSERT(State);
ASSERT(endpoint < c_Used_Endpoints);
GLOBAL_LOCK(irq);
struct AT91_UDPHS *pUdp = (struct AT91_UDPHS *) AT91C_BASE_UDP;
// If endpoint is not an output
if( State->Queues[endpoint] == NULL || !State->IsTxQueue[endpoint] )
return FALSE;
pUdp->UDPHS_IEN |= 1 << SHIFT_INTERUPT << endpoint;
pUdp->UDPHS_EPT[endpoint].UDPHS_EPTCTLENB = AT91C_UDPHS_TX_PK_RDY;
/* if the halt feature for this endpoint is set, then just
clear all the characters */
if(g_AT91_USBHS_Driver.EndpointStatus[endpoint] & USB_STATUS_ENDPOINT_HALT)
{
ClearTxQueue( State, endpoint );
return TRUE;
}
//If TxRunning, interrupts will drain the queue
if(!(BOOL)((UINT8)g_AT91_USBHS_Driver.TxRunning[endpoint]))
{
num_ep_int1++;
g_AT91_USBHS_Driver.TxRunning[endpoint] = TRUE;
TxPacket( State, endpoint );
}
else if(irq.WasDisabled()) // We should not call EP_TxISR after calling TxPacket becuase it can cause a TX FIFO overflow error.
{
Endpoint_ISR(endpoint);
}
return TRUE;
}
// read from ALL ax
void readPositionData(DynamixelComm *dc, byte* buffer, byte &failure)
{
printf("%s! %p\n", __func__, buffer);
int id, i;
// command: read AX12_DATA_READ bytes from address P_PRESENT_POSITION_L /ax12
gbpParameter[0] = P_PRESENT_POSITION_L; //Address
gbpParameter[1] = AX12_DATA_READ; //Read Length
// send command to every ax12 and store result in readBuffer[]
// failure = 1 signals a read failure
failure = AX12_NOERROR;
for(id=0; id<AX12_COUNT; id++)
{
failure = AX12_ERROR;
i = 0;
// on failure: retry AX12_RETRY times
while ( (failure == AX12_ERROR) && (i < AX12_RETRY))
{
TxPacket(dc, id + AX12_STARTID,INST_READ,2);
int len = RxPacket(dc, DEFAULT_RETURN_PACKET_SIZE+gbpParameter[1]);
//if(RxPacket(dc, DEFAULT_RETURN_PACKET_SIZE+gbpParameter[1]) == DEFAULT_RETURN_PACKET_SIZE+gbpParameter[1])
if(len == DEFAULT_RETURN_PACKET_SIZE+gbpParameter[1])
{
for (len=0; len<AX12_DATA_READ; len++) {
buffer[id*AX12_DATA_READ + len] = gbpRxBuffer[5 + len];
printf("%d:%x ", id*AX12_DATA_READ + len, buffer[id*AX12_DATA_READ + len]);
}
failure = AX12_NOERROR;
} else
{
i++;
}
}
if (failure == AX12_ERROR)
break;
}
printf("\n");
}
// ptp movement (sinus wave acceleration)
void writePositionDataPTPSinus(DynamixelComm *dc, unsigned long readPause, byte* readBuffer,byte* writeBuffer, float pos, float* tb, float* te, int* sgn, float* vmax, float* bmax)
{
int i;
long int current;
float se, t, tv;
// sync write AX12_DATA_WRITE bytes starting at address P_GOAL_POSITION_L
byte op = INST_SYNC_WRITE;
gbpParameter[0] = P_GOAL_POSITION_L; //Address
gbpParameter[1] = AX12_DATA_WRITE;
printf("%s joints %f: ", __func__, pos);
// prepare write buffer -> unpack data from serial and store bytewise in buffer
for (i=0;i<AX12_COUNT;i++)
{
gbpParameter[2 + i * (AX12_DATA_WRITE+1)] = i + 1;
current = readBuffer[i * AX12_DATA_WRITE + 0] + 256 * readBuffer[i * AX12_DATA_WRITE + 1];
t = pos * te[i];
tv = te[i] - tb[i];
printf("teb %f %f %f ", pos, bmax[i], vmax[i]);
if (t <= tb[i]) {
//printf("A");
se = bmax[i] * (0.25 * t * t + (tb[i] * tb[i] / (8.0 * M_PI * M_PI)) * (cos(2.0*M_PI*t/tb[i])-1.0));
} else if (t <= te[i] - tb[i]) {
//printf("B");
se = vmax[i] * (t - 0.5 * tb[i]);
} else {
//printf("C");
se = 0.5 * bmax[i] * (te[i] * (t + tb[i]) - 0.5*(t*t + te[i]*te[i] + 2.0 * tb[i]*tb[i]) + (tb[i]*tb[i]/(4.0 * M_PI * M_PI))*(1.0 - cos((2.0*M_PI/tb[i])*(t - tv))));
}
current += sgn[i] * (long int) (se);
if (current > 1023)
current = 1023;
else if (current < 0)
current = 0;
gbpParameter[3 + i * (AX12_DATA_WRITE+1) + 0] = current & 0xFF;
gbpParameter[3 + i * (AX12_DATA_WRITE+1) + 1] = (current & 0xFF00) >> 8;
//printf("%04x ", current);
if(sgn[i]*(long int)(se) != 0) {
//printf("%d/%f:%04lx:%04lx ", i, se, current, sgn[i] * (long int) (se));
printf("!!");
}
push_data(i, current);
/*
if (i == 13)
{
uart1_putc(current & 0xFF);
uart1_putc((current & 0xFF00) >> 8);
}
*/
// store target speed and torque in write buffer
gbpParameter[3 + i * (AX12_DATA_WRITE+1) + 2] = writeBuffer[i * AX12_DATA_WRITE + 2];
gbpParameter[3 + i * (AX12_DATA_WRITE+1) + 3] = writeBuffer[i * AX12_DATA_WRITE + 3];
gbpParameter[3 + i * (AX12_DATA_WRITE+1) + 4] = writeBuffer[i * AX12_DATA_WRITE + 4];
gbpParameter[3 + i * (AX12_DATA_WRITE+1) + 5] = writeBuffer[i * AX12_DATA_WRITE + 5];
if(!((writeBuffer[i * AX12_DATA_WRITE + 4] == 255) && (writeBuffer[i * AX12_DATA_WRITE + 5] == 3))) {
printf("%d:TORQUE FAIL!!",__LINE__);
}
}
printf("\n");
// send all data to ax12s AT ONCE
TxPacket(dc, BROADCASTING_ID,op,2+AX12_COUNT*(AX12_DATA_WRITE + 1));
}
void main()
{
UART_init();
//================NRF905初始化
CSN=1; // Spi disable
SCK=0; // Spi clock line init high
DR=1; // Init DR for input
AM=1; // Init AM for input
PWR_UP=1; // nRF905 power on
TRX_CE=0; // Set nRF905 in standby mode
TX_EN=0; // set radio in Rx mode
//================
Config905(); // Config nRF905 module
LED2=CD;
TxBuf[0] = 1;
TxBuf[1] = 1;
TxBuf[2] = 1;
TxBuf[3] = 1;
SetTxMode(); // Set Tx Mode
TxPacket(); // Transmit Tx buffer data
LED1 = 0;
Delay(500); // delay for led light
LED1 =1;
TxBuf[0] = 0xff;
TxBuf[1] = 0xff;
TxBuf[2] = 0xff;
TxBuf[3] = 0xff;
// led close
SetRxMode(); // Set nRF905 in Rx mode
while(1)
{
if(RECEIVR_buffer=='l'||RECEIVR_buffer=='r'||RECEIVR_buffer=='f'||RECEIVR_buffer=='b'||KEY1==0)
//if(KEY1 ==0 )
{ LED1 = 0;
TxBuf[0] = RECEIVR_buffer ;
COM_send(RECEIVR_buffer);
//===============NRF905发送相应的按键
SetTxMode(); // Set Tx Mode
TxPacket(); // Transmit Tx buffer data
//TxBuf[0] = 0xff;
//TxBuf[1] = 0xff;
SetRxMode(); // Set nRF905 in Rx mode
Delay(500);
LED1 = 1;
}
//=====================等待NRF905接收
if (DR) // If recive data ready... 数据准备就绪
RxPacket(); // ... recive data
}
}
bool BootpTx(void){
int i;
char *txPktBuf, *rxPktBuf;
long delay; // delay : 1초 delay후 실패면 재시도.
time_t now;
txPktBuf = PktBuf;
rxPktBuf = PktBuf;
bootpState = BOOTP_CONTINUE;
protocol = PROT_BOOTP;
// make boop packet.
MemSet(txPktBuf, 0, MAX_PKT_SIZE);
SetBootpHeader((char *)(txPktBuf+ETHER_HDR_SIZE+IP_HDR_SIZE+UDP_HDR_SIZE));
SetUdpHeader((char *)(txPktBuf+ETHER_HDR_SIZE+IP_HDR_SIZE), bootps, bootpc, BOOTP_HDR_SIZE);
SetIPHeader((char *)(txPktBuf+ETHER_HDR_SIZE), noIP, broadcastIP, UDP_HDR_SIZE+BOOTP_HDR_SIZE);
SetEtherHeader(txPktBuf, broadcastEther, PROT_IP);
//if (!NetInit()) return false;
// view info.
printf("Our Ethernet address : ");
PrintEthAddr(clientEther);
printf(".\n");
printf("Sending bootp packet...\n");
// bootp operation.
for (i=0; i<TIMEOUT; i++){
// transmit bootp packet to host.
printf(".");
if (!TxPacket(txPktBuf, ETHER_HDR_SIZE+IP_HDR_SIZE+UDP_HDR_SIZE+BOOTP_HDR_SIZE)) break;
// receive bootp packet from host.
delay = GetTime(&now)+HZ;
while (GetTime(&now)<delay && bootpState==BOOTP_CONTINUE){
RxPacket(rxPktBuf);
if (bootpState==BOOTP_SUCCESS) break;
}
if (bootpState==BOOTP_SUCCESS) break;
}
printf("\n");
protocol = NOPROTOCOL;
if (bootpState==BOOTP_SUCCESS){
printf("Bootp Packet received.\n");
printf("\tHost (server) Ethernet : ");
PrintEthAddr(hostEther);
printf("\n");
printf("\tHost (server) IP : ");
PrintIPAddr(hostIP);
printf("\n");
printf("\tClient (target) Ethernet : ");
PrintEthAddr(clientEther);
printf("\n");
printf("\tClient (target) IP : ");
PrintIPAddr(clientIP);
printf("\n");
printf("\n");
return true;
}
else {
printf("Bootp packet is not received.\n\n");
return false;
}
return true;
} // BootpTx.
