int main (void) {
DDRB |= 0xFF; // set all PORTB pins for output
//DDRB &= ~(_BV(PB2)); // set pin 16 for input
DDRC &= ~(_BV(PC0)); // set pin 30 for input
DDRD &= ~(_BV(PD6)); // set pin 14 for input
// Setting PE1 and PE2. XTAL1 to input and XTAL2 to ouput . Pins 10 and 11
DDRE |= _BV(PE2);
DDRE &= ~(_BV(PE1));
PORTB |= _BV(PB3)
sei(); // enable global interrupts
initCAN(NODE_HOME); // initialize CAN bus
initButton(); // intitialize button interrupts
for (;;) {
// listen for button presses forever
}
}
//-----------------------------------------------
void CANPeakSysUSB::init()
{
std::string sCanDevice;
if( m_IniFile.GetKeyString( "TypeCan", "DevicePath", &sCanDevice, false) != 0) {
sCanDevice = "/dev/pcan32";
} else std::cout << "CAN-device path read from ini-File: " << sCanDevice << std::endl;
//m_handle = LINUX_CAN_Open("/dev/pcan32", O_RDWR | O_NONBLOCK);
m_handle = LINUX_CAN_Open(sCanDevice.c_str(), O_RDWR);
if (! m_handle)
{
// Fatal error
std::cout << "Cannot open CAN on USB: " << strerror(errno) << std::endl;
sleep(3);
exit(0);
}
m_iBaudrateVal = 0;
m_IniFile.GetKeyInt( "CanCtrl", "BaudrateVal", &m_iBaudrateVal, true);
initCAN();
}
static void CAN_device_init_2(int baud)
{
CAN_DeInit(CAN2);
/* CAN GPIOs configuration ************************************************* */
/* Enable GPIO clock */
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);
/* Connect CAN pins to Alternate Function */
GPIO_PinAFConfig(GPIOB, GPIO_PinSource12, GPIO_AF_CAN2);
GPIO_PinAFConfig(GPIOB, GPIO_PinSource13, GPIO_AF_CAN2);
initGPIO(GPIOB, GPIO_Pin_12 | GPIO_Pin_13);
/* CAN configuration ******************************************************* */
/* Enable CAN clock */
RCC_APB1PeriphClockCmd(RCC_APB1Periph_CAN2, ENABLE);
initCAN(CAN2, baud);
/* Enable FIFO 0 message pending Interrupt */
CAN_ITConfig(CAN2, CAN_IT_FMP1, ENABLE);
initCANInterrupts(CAN2, CAN2_RX1_IRQn);
}
int main(void)
{
wiimote *wm;
wiimote** wiimotes;
int found, connected;
HANDLE canHandle;
TPCANMsg Message;
int wiimote_led_state;
int exit = 0;
canHandle = initCAN();
if(canHandle == NULL)
{
printf("Error opening CAN device!\n");
return -1;
}
wiimotes = wiiuse_init(MAX_WIIMOTES);
found = wiiuse_find(wiimotes, MAX_WIIMOTES, 5);
if (!found)
{
printf ("No wiimotes found.\n");
return 0;
}
connected = wiiuse_connect(wiimotes, MAX_WIIMOTES);
if (connected)
printf("Connected to %i wiimotes (of %i found).\n", connected, found);
else
{
printf("Failed to connect to any wiimote.\n");
return 0;
}
wm = wiimotes[0];
wiiuse_status(wm);
while(wm->event != WIIUSE_STATUS)
{
wiiuse_poll(wiimotes, MAX_WIIMOTES);
}
printf("Battery level: %f%%\n", wm->battery_level*100);
while (1)
{
if(exit)
break;
if (wiiuse_poll(wiimotes, MAX_WIIMOTES))
{
/*
* This happens if something happened on any wiimote.
* So go through each one and check if anything happened.
*/
int i = 0;
for (; i < MAX_WIIMOTES; ++i)
{
switch (wiimotes[i]->event)
{
case WIIUSE_EVENT:
/* a generic event occured */
handle_event(wiimotes[i]);
Message.ID = CAN_INPUT_MSG_ID;
Message.MSGTYPE = MSGTYPE_STANDARD;
Message.LEN = 7;
Message.DATA[0] = carInputs.accel;
Message.DATA[1] = carInputs.brake;
Message.DATA[2] = carInputs.steer;
Message.DATA[3] = carInputs.gear;
Message.DATA[4] = carInputs.clutch;
Message.DATA[5] = carInputs.controls;
Message.DATA[6] = carInputs.cruisedist;
CAN_Write(canHandle,&Message);
// Show the status of ABS/TC/Cruise on the LEDs
wiimote_led_state = 0;
if(carInputs.controls & ABS)
wiimote_led_state |= WIIMOTE_LED_1;
if(carInputs.controls & TC)
wiimote_led_state |= WIIMOTE_LED_2;
if(carInputs.controls & STABILITY)
wiimote_led_state |= WIIMOTE_LED_3;
if(carInputs.controls & CRUISE)
wiimote_led_state |= WIIMOTE_LED_4;
wiiuse_set_leds(wm, wiimote_led_state);
break;
case WIIUSE_DISCONNECT:
case WIIUSE_UNEXPECTED_DISCONNECT:
/* the wiimote disconnected */
handle_disconnect(wiimotes[i]);
exit = 1;
break;
default:
break;
}
}
}
}
wiiuse_cleanup(wiimotes, MAX_WIIMOTES);
return 0;
}
void main(void) {
unsigned char swTrig = 0;
byte l3 = 1;
lDelay();
Wait4NN = FALSE;
isLearning = FALSE;
led1timer = 0;
doSOD = 0;
ioIdx = 0;
doEV = 0;
evIdx = 0;
NV1 = eeRead(EE_NV);
initIO();
resetOutputs();
NN_temp = eeRead(EE_NN) * 256;
NN_temp += eeRead(EE_NN + 1);
if (NN_temp == 0 || NN_temp == 0xFFFF)
NN_temp = DEFAULT_NN;
CANID = eeRead(EE_CANID);
if (CANID == 0 || CANID == 0xFF)
CANID = NN_temp & 0xFF;
initCAN();
delay();
restoreOutputStates();
delay();
SOD = eeRead(EE_SOD) * 256;
SOD += eeRead(EE_SOD + 1);
if (SOD == 0 || SOD == 0xFFFF)
SOD = DEFAULT_SOD;
// Loop forever (nothing lasts forever...)
while (1) {
CANMsg cmsg;
unsigned char txed = 0;
LED3 = PORT_ON;
l3 ^= 1;
// Check for Rx packet and setup pointer to it
while (canbusRecv(&cmsg)) {
// Decode the new command
LED1 = 1;
led1timer = 20;
txed = parseCmd(&cmsg);
}
LED3 = PORT_OFF;
doTimedOff(ioIdx);
if (checkInput(ioIdx, doSOD)) {
ioIdx++;
if (ioIdx >= 16) {
ioIdx = 0;
doSOD = 0;
}
}
if (l3) {
if (doPortEvent(evIdx)) {
evIdx++;
if (evIdx >= 16) {
evIdx = 0;
doEV = 0;
}
}
}
if (checkFlimSwitch() && !swTrig) {
swTrig = 1;
} else if (!checkFlimSwitch() && swTrig) {
swTrig = 0;
if (Wait4NN) {
Wait4NN = 0;
LED2 = 0;
} else {
CANMsg canmsg;
LED2 = 1;
canmsg.b[d0] = OPC_RQNN;
canmsg.b[d1] = NN_temp / 256;
canmsg.b[d2] = NN_temp % 256;
canmsg.b[dlc] = 3;
canbusSend(&canmsg);
Wait4NN = 1;
}
}
}
}
/** Load information from configuration files, initialize structures, and open CAN device drivers
\return - 0 Success
- -1
- -2 Error parsing the config file
*/
int InitializeSystem(void)
{
int err, id;
int bus_number;
char key[256], valStr[256];
//int num_buses;
int canAddr;
char ethAddr[32];
long status[MAX_NODES];
long reply;
int groupID;
int orderIdx;
int cnt, cnt2, cnt3, cnt4;
int i,j,k,l;
int min, max, min_idx, max_idx, act_cnt, grp_cnt;
int group_cnt[65];
//actuator_struct *act;
/* Goals:
Initialize and enumerate each bus (bus_struct buses should already be filled in).
Fill in actuator/motor/puck data structures
*/
// Parse config file
//err = parseFile(fn);
//if (err)
// return -2;
// For each bus, initialize
//err = parseGetVal(INT, "system.busCount", (void*)&num_buses);
//syslog(LOG_ERR, "Bus count = %d", num_buses);
cnt = 0;
for(bus_number = 0; bus_number < num_buses; bus_number++) {
// Initialize the bus[] structure
buses[bus_number].num_pucks = 0;
buses[bus_number].num_groups = 0;
for (groupID = 0; groupID < 65; groupID++) {
buses[bus_number].group[groupID].group_number = -1;
for (orderIdx = 0; orderIdx < 4; orderIdx++) {
buses[bus_number].group[groupID].pucks_by_order[orderIdx] = -1;
}
}
// Get bus device string
//sprintf(key, "system.bus[%d].device", bus_number);
//err = parseGetVal(STRING, key, (void*)(buses[bus_number].device_str));
// Get bus type
//sprintf(key, "system.bus[%d].type", bus_number);
//err = parseGetVal(STRING, key, (void*)valStr);
// Get bus address
//sprintf(key, "system.bus[%d].address", bus_number);
// If this is a CANbus
if(buses[bus_number].type == CAN) {
//err = parseGetVal(INT, key, (void*)&canAddr);
// Initialize the hardware channel
if(err = initCAN(bus_number, buses[bus_number].address))
syslog(LOG_ERR, "Could not initialize can bus %d, err = %d", bus_number, err);
//wakePuck(bus_number, GROUPID(WHOLE_ARM));
// Enumerate nodes on the bus
err = getBusStatus(bus_number, status);
for(id = 0; id < MAX_NODES; cnt += status[id++] >= 0)
; // Count responding nodes
// If this is an Ethernet bus
} else if(buses[bus_number].type == ETHERNET) {
//err = parseGetVal(STRING, key, (void*)ethAddr);
syslog(LOG_ERR, "Robot on bus %d not initialized, Ethernet not supported", bus_number);
}
// Else, bus type not valid
else {
syslog(LOG_ERR, "InitializeSystem: Invalid bus type (%d)", buses[bus_number].type);
}
}
/* Allocate the actuators data structure */
syslog(LOG_ERR, "About to allocate space for %d nodes", cnt);
act = malloc(sizeof(actuator_struct) * (cnt + 3));
if (act == NULL) {
syslog(LOG_ERR, "Actuator Memory allocation failed. Tried to allocate %d actuators.", cnt);
return -1;
}
num_actuators = 0;
for(bus_number = 0; bus_number < num_buses; bus_number++) {
safetyBoardID[bus_number] = 0;
err = getBusStatus(bus_number, status);
// Query each node for ID, CPR, IPNM, PIDX, GRPx
firstPos[bus_number] = TRUE;
for(id = 0; id < MAX_NODES; id++) {
invalidPos[id] = 0;
insanePos[id] = 0;
if(status[id] == -1) continue; // If there is no puck here, move on
getProperty(bus_number, id, ROLE, &reply);
if((reply & 0x000F) == ROLE_SAFETY) {
if(status[id] != STATUS_READY) {
syslog(LOG_ERR, "The safety module on bus %d is not functioning properly", bus_number);
}else{
safetyBoardID[bus_number] = id;
}
} else {
switch(status[id]) {
case STATUS_RESET: // Get the puck out of reset
syslog(LOG_ERR, "Waking puck %d", id);
wakePuck(bus_number, id);
case STATUS_READY:
// Make sure the puck is in IDLE mode
setProperty(bus_number, id, MODE, FALSE, MODE_IDLE);
usleep(250000);
// Assign the bus
act[num_actuators].bus = bus_number;
// Query for various info
act[num_actuators].puck.ID = id;
switch(id) {
case -1: //case 1: case 4: // xxx Remove me
act[num_actuators].motor.counts_per_rev = 40960;
act[num_actuators].motor.puckI_per_Nm = 2755;
act[num_actuators].puck.order = id-1;
act[num_actuators].puck.group = 1;
break;
default:
getProperty(bus_number, id, CTS, &reply);
act[num_actuators].motor.counts_per_rev = reply;
getProperty(bus_number, id, IPNM, &reply);
act[num_actuators].motor.puckI_per_Nm = reply;
getProperty(bus_number, id, PIDX, &reply);
act[num_actuators].puck.order = reply-1;
getProperty(bus_number, id, GRPB, &reply);
act[num_actuators].puck.group = reply;
break;
}
syslog(LOG_ERR,"Puck: ID=%d CTS=%d IPNM=%.2lf PIDX=%d GRPB=%d",
act[num_actuators].puck.ID,
act[num_actuators].motor.counts_per_rev,
act[num_actuators].motor.puckI_per_Nm,
act[num_actuators].puck.order,
act[num_actuators].puck.group);
// Set MaxTorque to 3.3A
// 2.4A = 1/4 breaking strength = 3441
switch(id) {
case 1:
case 2:
case 3:
setProperty(bus_number, id, MT, FALSE, 4860); // Cable limit = 4860
break;
case 4:
setProperty(bus_number, id, MT, FALSE, 4320); // Cable limit = 4320
break;
case 5:
case 6:
setProperty(bus_number, id, MT, FALSE, 3900); // Cable limit = 3900
break;
case 7:
setProperty(bus_number, id, MT, FALSE, 1370); // Max continuous stall
break;
default:
setProperty(bus_number, id, MT, FALSE, 1370);
break;
}
++num_actuators; // Update the number of actuators
break;
default:
break;
}
}
}
}
// Get the cycles per second on this machine
CPU_cycles_per_second = sysconf(_SC_CLK_TCK);
for (cnt = 0; cnt < num_actuators; cnt++) {
act[cnt].puck.position = 0;
act[cnt].puck.torque_cmd = 0;
act[cnt].angle = 0;
act[cnt].torque = 0;
act[cnt].puck.zero = 0;
act[cnt].puck.index = 0;
act[cnt].UseTRC = 0;
act[cnt].UseEngrUnits = 1;
}
//set up group indexes. CAVEATS: does not check for duplicate order numbers in groups
for (i = 0; i < num_buses; i++) {
buses[i].total_pucks=0; /** \bug This variable seems unused */
//initialize the group counter
for (l = 0;l < 65;l++)
group_cnt[l] = 0;
//get number of actuators on this bus
act_cnt = 0;
for (j = 0; j < num_actuators; j++) {
if (act[j].bus == i) {
act_cnt++;
group_cnt[act[j].puck.group]++;
}
}
buses[i].num_pucks = act_cnt;
//figure out how many groups were found
for (l = 0;l < 65;l++) {
if (group_cnt[l]) {
buses[i].num_groups++;
buses[i].group[buses[i].num_groups - 1].group_number = l;
}
}
min = 0;
//for each actuator on this bus scan through and sort from lowest puck_id to highest
for (j = 0;j < act_cnt; j++) {
max = 1000;
for (k = 0; k < num_actuators; k++) {
if ((act[k].bus == i) && (act[k].puck.ID < max) && (act[k].puck.ID > min)) {
max = act[k].puck.ID;
max_idx = k;
}
}
buses[i].pucks_by_id[j] = max_idx;
min = max;
}
//zero out pucks_by_order in index
for (k = 0;k < buses[i].num_groups;k++) {
for (l = 0; l < 4;l++)
buses[i].group[k].pucks_by_order[l] = -1;
}
//create index of pucks by order
for (j = 0;j < buses[i].num_pucks;j++) {
for (k = 0;k < buses[i].num_groups;k++) {
if (act[buses[i].pucks_by_id[j]].puck.group == buses[i].group[k].group_number) {
buses[i].group[k].pucks_by_order[act[buses[i].pucks_by_id[j]].puck.order % 4] = buses[i].pucks_by_id[j];
}
}
}
}
// Success!
act_data_valid = 1;
DumpData2Syslog();
return 0;
}
int main(void)
{
initUART(UBBR);
printf("Simple RAM TEST \r\n");
initExtMemIface();
initTimer();
initInterrupts();
SRAM_test();
joyInit();
initOLED();
initSPI();
initCAN(NORMAL);
struct joypos_t p;
enum joydir_t d;
struct canMessage m0,m1,m2s;
while(1){
/* slide1Point=getSlidePosition(1);
slide2Point=getSlidePosition(2);
printf("SLIDE1 -> %d",slide1Point.x);
printf("SLIDE2 -> %d",slide2Point.x);SLIDE READINGS*/
if (readControl) {
readControl = 0;
d = getJoyDirection();
p = getJoyPosition();
sendCANJoy(2,p,d);
if (d == TOP || d == RIGHT_TOP || d == LEFT_TOP) menuOption = (menuOption-1)%NUM_MENU_OPTIONS;
else if (d == BOTTOM || d == RIGHT_BOTTOM|| d == LEFT_BOTTOM) menuOption = (menuOption+1)%NUM_MENU_OPTIONS;
printMenu(menuOption);
}
if (flagJoyButton) {
m2s.id = JOY_BUTTON;
m2s.size = 0;
printf("Sending joy bu\r\n");
fillTxBufferMCP(0,m2s);
requestToSendMCP(0);
struct canMessage m2s;
flagJoyButton = 0;
gotoCharOLED(7,0);
putsOLED(" "); //Cleans last selected option from screen
switch(menuOption){
case 0: //Play
gotoCharOLED(7,0);
putsOLED("PLAY");
break;
case 1: //Options
gotoCharOLED(7,0);
putsOLED("OPTIONS");
break;
case 2: //Help
printHelp();
break;
default:
gotoCharOLED(7,0);
putsOLED("Stick to the OPTIONS !!!");
break;
}
}
/*
if(flagMCP) {
flagMCP = 0;
struct canMessage m0,m1;
m0 = readRxBufferMCP(0);
m1 = readRxBufferMCP(1);
printf ("Buffer 0 ID: %d, Size: %d, Data: %s \r\n", m0.id, m0.size, m0.data);
printf ("Buffer 1 ID: %d, Size: %d, Data: %s \r\n", m1.id, m1.size, m1.data);
}*/
}
}
void main(void)
{
unsigned int i,sweep=0;
DAC_DATA data;
byte test;
BYTE bdata;
deassertCSB0();
deassertCSB1();
deassertCSB2();
deassertCSB3();
deassertCSB4();
deassertCSB5();
deassertCSB6();
openFulldSPI();
openHalfdSPI();
//initTERM();
initCAN();
initDAC(0);
resetCLKD();
resetADC();
//Configuring AD9510 PLL Loop.
//In Fixed Divider mode (for N feedback divider), we have:
//Fvco = Fref * N/R
//Thus, N/R = 2 for Fvco = 125 MHz and Fref = 62.5 MHz.
//Where N is the feedback divider and R is the reference divider.
//Let N = 2 and R = 1. See below.
//R divider (Reference divider)
//R divider is set to '1'.
writeCLKD(0x0C, 0x01);
writeCLKD(0x0B, 0x00);
//N divider (Vco feedback divider)
//N divider: N(P, A, B).
//N = P*B+A.
//P is a prescaler (3 bits).
//A and B are counters, 6 and 3 bits respectively.
//Two modes are possible: FD (fixedd divider) and DM (dual modulus).
//In FD mode, A is not used.
//Set P prescaler to 2 and set B counter to 1 (bypass). Also set PLL to Normal Operation.
writeCLKD(0x0A, 0x44);
writeCLKD(0x07, 0x04); //Enable Loss of Reference (LOR) function.
writeCLKD(0x08, 0x0F); //Set Normal Operation mode to CP and STATUS Pin to 'Loss of Lock (active high)'.
//writeCLKD(0x08, 0x0D); //Set Pump-up mode to CP and STATUS Pin to 'Loss of Lock (active high)'.
//writeCLKD(0x08, 0x37); //Set Normal Operation mode to CP and STATUS Pin to 'Loss of Lock or Loss of Ref (active high)'.
//writeCLKD(0x08, 0x2B); //Set Normal Operation to CP and STATUS Pin to 'Loss of Ref (active high)'.
//writeCLKD(0x08, 0x35); //Set Pump-up mode to CP and STATUS Pin to 'Loss of Lock or Loss of Ref (active high)'.
//writeCLKD(0x09, 0x40); //Set CP current to 3.0 mA.
writeCLKD(0x09, 0x00); //Set CP current to 0.6 mA.
writeCLKD(0x45, 0x02); //Set CLK2 as Distribution input, power down CLK1 input. See doc.
writeCLKD(0x42, 0x02); //Enable OUT6
writeCLKD(0x43, 0x02); //Enable OUT7
writeCLKD(0x4B, 0xC0); //(OUT1) Set 'Ignore Chip-Level Sync Signal' and 'Bypass and Power-Down Divider Logic'. See doc.
writeCLKD(0x51, 0xC0); //(OUT4 - ADC78) Set 'Ignore Chip-Level Sync Signal' and 'Bypass and Power-Down Divider Logic'. See doc.
writeCLKD(0x53, 0xC0); //(OUT5 - ADC56) Set 'Ignore Chip-Level Sync Signal' and 'Bypass and Power-Down Divider Logic'. See doc.
writeCLKD(0x55, 0xC0); //(OUT6 - ADC34) Set 'Ignore Chip-Level Sync Signal' and 'Bypass and Power-Down Divider Logic'. See doc.
writeCLKD(0x57, 0xC0); //(OUT7 - ADC12) Set 'Ignore Chip-Level Sync Signal' and 'Bypass and Power-Down Divider Logic'. See doc.
//writeCLKD(0x57, 0x40); //(OUT7 - ADC12) Divider Logic Enabled - Dividing by 2 (Default) - Phase 0.
//writeCLKD(0x55, 0x41); //(OUT6 - ADC34) Divider Logic Enabled - Dividing by 2 (Default) - Phase 1.
//test = readCLKD(0x45);
writeCLKD(0x5A, 0x01); //Update Registers with SPI Buffers Content.
/*********************************/
//for ADC12
writeADC(1, 0x08, 0x00); //0x00: Normal op. - 0x01: Full Power Down.
writeADC(1, 0x14, 0xC1); //LVDS and two's comlement.
writeADC(1, 0x0D, 0x00); //Self Test - 0x00: Normal op. - 0x07: one/zero word toggle.
writeADC(1, 0xFF, 0x01); //Master Register latch enable - self resetable.
//for ADC34
writeADC(2, 0x08, 0x00);
writeADC(2, 0x14, 0xC1);
writeADC(2, 0xFF, 0x01);
//for ADC56
writeADC(4, 0x08, 0x00);
writeADC(4, 0x14, 0xC1);
writeADC(4, 0xFF, 0x01);
//for ADC78
writeADC(3, 0x08, 0x00); //0x01
writeADC(3, 0x14, 0xC1);
writeADC(3, 0xFF, 0x01);
data.wdata = 512; //~313mV
writeDAC(5, 0, data);
writeDAC(5, 1, data);
writeDAC(5, 2, data);
writeDAC(5, 3, data);
writeDAC(5, 4, data);
writeDAC(5, 5, data);
writeDAC(5, 6, data);
writeDAC(5, 7, data);
for(;;)
{
//test = readCLKD(0x45);
//writeCLKD(0x5A, 0x01);
//bdata._byte = 0xAA;
//writeHSPI(bdata);
for(i=0;i<10000;i++);
//data.wdata = sweep;
//writeDAC(5, 0, data);
//writeDAC(6, 0, data);
//sweep++;
//if (sweep>4095) sweep = 0;
//rxCAN();
}
//for(;;);
}
