int main(void)
{
uint8_t TWIS_ResponseType;
init_modi();
init_io_ports();
init_timer();
set_outputs();
/*
** Start TWI Slave with address 15 and bitrate of 100000 Hz
*/
TWIS_Init();
/*
mainloop - Kommunikation mit dem master
*/
while (1)
{
if (TWIS_ResponseRequired(&TWIS_ResponseType))
{
switch (TWIS_ResponseType)
{
/*
** Slave is requested to read bytes from the master.
*/
case TW_SR_SLA_ACK:
{
outputdata.ports = TWIS_ReadAck();
outputdata.ports += TWIS_ReadAck() << 8;
uint8_t i;
for (i = 0; i < PWM_CHAN - 1; i++)
outputdata.pwmval[i] = TWIS_ReadAck();
outputdata.pwmval[i] = TWIS_ReadNack();
TWIS_Stop(); // I2C stop
set_outputs();
}
break;
/*
** Slave is requested to send bytes from the master.
*/
case TW_ST_SLA_ACK:
TWIS_Write(outputdata.ports);
TWIS_Write(outputdata.ports >> 8);
for (int read_p = 0; read_p < PWM_CHAN; read_p++)
TWIS_Write(outputdata.pwmval[read_p]);
TWIS_Stop();
break;
default:
TWIS_Stop();
break;
}
}
wdt_reset();
}
}
// M1 left
// M2 right
void motorInit(){
// Keep pins low if unused.
set_outputs(M1reverse, M1enable, 0b0000);
set_outputs(M2reverse, M2enable, 0b0000);
set_directions(M1reverse, M1enable, 0b1111);
set_directions(M2reverse, M2enable, 0b1111);
}
static void write_eeprom(struct usb_dev_handle *usb_handle, int addr, unsigned short data)
{
unsigned char buf[4];
buf[0] = 0x80;
buf[1] = data & 0xff;
buf[2] = data >> 8;
buf[3] = 0xc0 | (addr & 0x3f);
set_outputs(usb_handle,buf);
}
int getQTIs(int highPin, int lowPin) // Function - getQTIs
{
int dt = (CLKFREQ / 1000000) * 230; // Set up 230 us time increment
set_outputs(highPin, lowPin, 0b1111); // highPin...lowPin set high
set_directions(highPin, lowPin, 0b1111); // highPin...lowPin set to output
waitcnt(dt + CNT); // Wait 230 us
set_directions(highPin, lowPin, 0b0000); // highPin...lowPin st to input
waitcnt(dt + CNT); // Wait 230 us
int qtis = get_states(highPin, lowPin); // Get 4-bit pattern QTIs apply
return qtis; // Return val containing pattern
}
static void setout(struct usb_dev_handle *usb_handle,unsigned char c)
{
unsigned char buf[4];
buf[0] = buf[3] = 0;
if (devtype == DEV_C119) buf[2] = 0x3d; /* set GPIO 1,3,4,5,6 as output */
else buf[2] = 0xd; /* set GPIO 1,3,4 as output */
buf[1] = c; /* set GPIO 1,3,4 (5,7) outputs appropriately */
set_outputs(usb_handle,buf);
usleep(100000);
}
static unsigned short read_eeprom(struct usb_dev_handle *usb_handle, int addr)
{
unsigned char buf[4];
buf[0] = 0x80;
buf[1] = 0;
buf[2] = 0;
buf[3] = 0x80 | (addr & 0x3f);
set_outputs(usb_handle,buf);
memset(buf,0,sizeof(buf));
get_inputs(usb_handle,buf);
return(buf[1] + (buf[2] << 8));
}
int main(void)
{
sys_init(); // Initialize system
canInit(CAN_BAUDRATE); // Initialize the CANopen bus
initTimer(); // Start timer for the CANopen stack
nodeID = read_bcd(); // Read node ID first
setNodeId (&ObjDict_Data, nodeID);
setState(&ObjDict_Data, Initialisation); // Init the state
for(;;) // forever loop
{
if (sys_timer) // Cycle timer, invoke action on every time slice
{
reset_sys_timer(); // Reset timer
digital_input[0] = get_inputs();
digital_input_handler(&ObjDict_Data, digital_input, sizeof(digital_input));
digital_output_handler(&ObjDict_Data, digital_output, sizeof(digital_output));
set_outputs(digital_output[0]);
// Check if CAN address has been changed
if(!( nodeID == read_bcd()))
{
nodeID = read_bcd(); // Save the new CAN adress
setState(&ObjDict_Data, Stopped); // Stop the node, to change the node ID
setNodeId(&ObjDict_Data, nodeID); // Now the CAN adress is changed
setState(&ObjDict_Data, Pre_operational); // Set to Pre_operational, master must boot it again
}
}
// a message was received pass it to the CANstack
if (canReceive(&m)) // a message reveived
canDispatch(&ObjDict_Data, &m); // process it
else
{
// Enter sleep mode
#ifdef WD_SLEEP // Watchdog and Sleep
wdt_reset();
sleep_enable();
sleep_cpu();
#endif // Watchdog and Sleep
}
}
}
static float ReadUltrasonic(uint8_t addr)
{
// Set pins directions
set_direction(MUX_ADDR_START, OUTPUT);
set_direction(MUX_ADDR_START + 1, OUTPUT);
set_direction(MUX_ADDR_START+ 2, OUTPUT);
set_direction(MUX_ADDR_START + 3, OUTPUT);
set_direction(TRIG_PIN, OUTPUT);
set_direction(ECHO_PIN, INPUT);
// Set the mux based on sensor index
set_outputs(MUX_ADDR_END, MUX_ADDR_START, (int) addr);
// Pulse the trigger pin
low(TRIG_PIN);
pulse_out(TRIG_PIN, 10);
// Read in echo pulse
uint32_t pulse = pulse_in(ECHO_PIN, HIGH);
return pulse/58.0;
}
int main(int argc, char **argv)
{
struct uterm_video *video;
int ret;
unsigned int mode;
const char *node;
ret = test_prepare(argc, argv, &eloop);
if (ret)
goto err_fail;
if (conf_global.use_fbdev) {
mode = UTERM_VIDEO_FBDEV;
node = "/dev/fb0";
} else {
mode = UTERM_VIDEO_DRM;
node = "/dev/dri/card0";
}
log_notice("Creating video object using %s...", node);
ret = uterm_video_new(&video, eloop, mode, node);
if (ret) {
if (mode == UTERM_VIDEO_DRM) {
log_notice("cannot create drm device; trying dumb drm mode");
ret = uterm_video_new(&video, eloop,
UTERM_VIDEO_DUMB, node);
if (ret)
goto err_exit;
} else {
goto err_exit;
}
}
log_notice("Wakeing up video object...");
ret = uterm_video_wake_up(video);
if (ret < 0)
goto err_unref;
if (argc < 2) {
ret = list_outputs(video);
if (ret) {
log_err("Cannot list outputs: %d", ret);
goto err_unref;
}
} else {
ret = uterm_video_use(video);
if (ret)
ret = blit_outputs(video);
else
ret = set_outputs(video);
if (ret) {
log_err("Cannot set outputs: %d", ret);
goto err_unref;
}
}
err_unref:
uterm_video_unref(video);
err_exit:
test_exit(eloop);
err_fail:
test_fail(ret);
return abs(ret);
}
