int setup(void) {
// Optimize system timings
SYSTEMConfig(SYSCLK, SYS_CFG_WAIT_STATES | SYS_CFG_PCACHE);
// Setup interrupts
//INTConfigureSystem(INT_SYSTEM_CONFIG_MULT_VECTOR);
//INTEnableInterrupts();
// Setup LEDs, these pins need to be outputs
TRISEbits.TRISE0 = 0;
TRISEbits.TRISE1 = 0;
TRISEbits.TRISE2 = 0;
TRISEbits.TRISE3 = 0;
// Set all analog pins to be digital I/O
AD1PCFG = 0xFFFF;
//Configure SPI
SpiChnOpen(1, SPI_CON_MSTEN | SPICON_CKE | SPI_CON_MODE8 | SPI_CON_ON, 200);
PORTSetPinsDigitalIn(IOPORT_E, BIT_4);
//PORTSetPinsDigitalIn(IOPORT_D, BIT_10);
//PORTSetPinsDigitalIn(IOPORT_D, BIT_0);
//PORTSetPinsDigitalIn(IOPORT_C, BIT_4);
int rData = SPI1BUF; //Clears receive buffer
IFS0CLR = 0x03800000; //Clears any existing event (rx / tx/ fault interrupt)
SPI1STATCLR = 0x40; //Clears overflow
ad7466_cs = 1; //Make sure CS pin is high
YLED = 1; //1 is off, 0 is on
RLED = 1;
WLED = 1;
GLED = 1;
setupSerial();
SendDataBuffer("Radiometer Configuration Complete.\n\r", sizeof("Radiometer Configuration Complete.\n\r"));
return 0;
}
gint connectSerial() {
#ifdef DEBUG
printf("Connecting to device %s\n", DEFAULT_DEVICE);
#endif
if (fd >= 0) {
return setupSerial(device);
}
return -1;
}
void SSS::begin(){
Wire.begin();
setupSerial();
setPinModes();
displayCAN = false;
separatorChar = ',';
numCommands = 83;
numCANmsgs = 0;
displayCAN = false;
len = 0;
_ignitionPin = 5;
}
int main( int argc, char *argv[]){
/* parse options */
int option_index = 0, opt;
int gui = 1;
static struct option loptions[] = {
{"help", no_argument, 0, 'h'},
{"port", required_argument, 0, 'p'},
{"nongui", required_argument, 0, 'n'},
{"quiet", no_argument, 0, 'q'},
{0, 0, 0, 0}
};
do {
opt = getopt_long (argc, argv, "hp:nq",
loptions, &option_index);
switch (opt) {
case '0':
break;
case 'p':
device = strdup(optarg);
break;
case 'n':
gui = 0;
run = 1;
break;
case 'h':
usagePrint();
break;
//default:
}
} while(opt != -1);
if (device == NULL) {
device = strdup(DEFAULT_DEVICE);
}
fd = setupSerial(device);
if (gui) {
return doGUI(argc, argv);
} else {
return doConsole(argc, argv);
}
return 0;
}
int main(void)
{
// Call setup functions
setupSerial();
setupTimers();
setupADC();
// Set external interrupts
sei();
while (1)
{
// Infinite loop
}
return 0;
}
//--------------------------------------------------------------
void testApp::setup(){
// determine autoPilot and Serial port
autoPilot = !setupSerial();
// add a listener for the Canvas object
ofAddListener(serialEvent, &c, &Canvas::onSerialEvent);
// screen setup
//ofSetCircleResolution(64);
ofSetBackgroundAuto(false);
ofSetVerticalSync(true);
ofEnableSmoothing();
ofEnableAlphaBlending();
// other setup
ofSetRectMode(OF_RECTMODE_CENTER);
// for manual control without box
digitalVal = 0;
for(int i=0; i<6; i++){
analogVals[i] = 0;
digitalVals[i] = 0;
}
}
//#pragma code highPriorityInterruptAddress=0x0008
//void high_interrupt(void)
//{
// _asm GOTO highPriorityIsr _endasm
//}
//
//#pragma code
void main(void){
//Default
TRISD = 0x00; //set port D signals to output
menu_ref_1=1;
menu_ref_2=0;
/*zone of disgust*/
values[MAXSPEED]=100; //Max speed
values[PIDGAINS]=100; //PID Gains
values[MAXYAW]=100; //Max Yaw
values[IRSAMPE]=10; //ir_samp_e;
values[IRSAMPR]=20; //ir_samp_r;
values[IRRAW]=60; //ir_raw
values[IRAVG]=10; //ir_avg;
/*****************/
setupSerial();
Lcd_Init();
ADC_setup();
Button_Setup();
welcome();
LCD_disp(menu_ref_1, menu_ref_2);
RUN=0;
while(1){
//NORMAL OPERATION
while(RUN==0){
if(PORTDbits.RD0 == 0)
{
mode_button = 1;
}
if(PORTDbits.RD1 == 0)
{
motor_button = 1;
}
if (mode_button==1){ //Mode change sequence
checkMode(menu);
menu_ref_2=0; //Default menu ref, should only be SPEED
delayms(200);
mode_button=0;
LCD_disp(menu_ref_1, menu_ref_2);
}
delayms(200); //Used to prevent double tapping
//possibly new function
switchChannels(0);
joy_x = doADC();
switchChannels(1);
joy_y = doADC();
if(menu_ref_1==MANUAL) //IF IN MANUAL
{
if(joy_x<=LEFT){ //User pushes right joystick left
if(values[menu_ref_2]>0){
values[menu_ref_2]=values[menu_ref_2]-5; //decrement value by 5%
GLOBAL_MAX_SPEED = values[menu_ref_2];
sendMaxSpeed();
}
LCD_disp(menu_ref_1, menu_ref_2);
delayms(200); //Delay to prevent flickering, and also to prevent 100-0 increments
}
else if (joy_x>=RIGHT){ //User pushes right joystick right
if(values[menu_ref_2]<100){
values[menu_ref_2]=values[menu_ref_2]+5; //increment value by 5%
GLOBAL_MAX_SPEED = values[menu_ref_2];
sendMaxSpeed();
}
LCD_disp(menu_ref_1, menu_ref_2);
delayms(200);
}
}
else if(menu_ref_1==FACTORY){ //IF IN FACTORY MODE
if (joy_y>=UP){ //User pushes left joystick UP
//Circular selection
if(menu_ref_2==0){
menu_ref_2=4;
}
else{
menu_ref_2--; //switched the order of this to prevent overflow
}
LCD_disp(menu_ref_1, menu_ref_2);
delayms(250); //Arbitrary delay of 250 milliseconds
}
else if (joy_y<=DOWN){ //User pushes left joystick DOWN
menu_ref_2++;
//Circular selection
if(menu_ref_2>=7){
menu_ref_2=1;
}
LCD_disp(menu_ref_1, menu_ref_2);
delayms(250);
}
else if (joy_x<=LEFT){ //User pushes right joystick left
if(values[menu_ref_2]>0){
values[menu_ref_2]=values[menu_ref_2]-5; //decrement value by 5%
}
LCD_disp(menu_ref_1, menu_ref_2);
delayms(255);
delayms(200);
}
else if (joy_x>=RIGHT){ //User pushes right joystick right
if(values[menu_ref_2]<100){
values[menu_ref_2]=values[menu_ref_2]+5; //decrement value by 5%
}
LCD_disp(menu_ref_1, menu_ref_2);
delayms(255);
delayms(200);
}
}
//All other menu_ref_1's do not have values displayed
if (motor_button==1){
motor_button=0;
RUN=1;
}
//send here
}
/*Menu on setup*/
if (RUN==1){
on_setup();
GLOBAL_RUN = RUN;
sendRun();
//MOTOR ON GOES AFTER
motor_button=0;
}
/*MOTOR ON BEHAVIOUR*/
while(RUN==1){
if(PORTDbits.RD1 == 0)
{
motor_button = 1;
}
if (motor_button==1){ //At interrupt, stop motor and return to menu
motor_button=0;
RUN=0;
GLOBAL_RUN = RUN;
sendRun();
}
switchChannels(0);
//GLOBAL_VELOCITY = doADC(); GLOBAL_VELOCITY = doADC() //Get joystick values
joy_y = doADC();
GLOBAL_VELOCITY = joy_y;
switchChannels(1);
joy_x = doADC();
GLOBAL_OMEGA = joy_x;
sendVelocity();
sendOmega();
//send command here
}
/*MOTOR OFF MESSAGE*/
RUN=0;
//MOTOR OFF FUNCTION GOES BEFORE MESSAGE
LCD_title(t5); //"Whoa!"
delays(2); //safety delay
LCD_disp(menu_ref_1, menu_ref_2); //Return to normal display
}
}
void main(void)
{
PWM_motor_setup();
ADC_setup();
setupSerial();
IR_Setup();
TMR2_INT_setup();
while(1)
{
while(GLOBAL_MODE == MANUAL)
{
//turn(GLOBAL_VELOCITY,GLOBAL_OMEGA);
noob_turn(GLOBAL_VELOCITY,GLOBAL_OMEGA);
}
while(GLOBAL_MODE == ASSIST)
{
IR_Calculate();
if(dist_count == 2)
{
new_turn(5, Get_dist_diff());
newdistance = 0;
}
}
while(GLOBAL_MODE == AUTO)
{
IR_Calculate();
if(newstate == 1)
{
if(Get_S_IR_state() == IR_TOO_CLOSE)
{
// turn(AUTO_VEL, AUTO_OMEGA);
CCPR1L = 40;
CCPR2L = 50;
}
else if(Get_S_IR_state() == IR_TOO_FAR)
{
// turn(AUTO_VEL,255 -AUTO_OMEGA);
CCPR1L = 50;
CCPR2L = 40;
}
else if(Get_S_IR_state() == IR_ERROR)
{
turn(128,128);
}
newstate = 0;
}
}
}
}
int main( int argc, char* argv[] ) {
setbuf(stdout, NULL);
INFO("%s v1.0\n", argv[0]);
config_t cfg;
global_data_t state;
struct mosquitto *mosq = NULL;
config_init(&cfg);
loadDefaults(&cfg);
loadSerialDefaults(&cfg);
if(parseArgs(argc, argv, &cfg)) {
exit(1);
}
if(parseSerialArgs(argc, argv, &cfg)) {
exit(1);
}
//config_write(&cfg, stderr);
port_t serial;
loadSerial(&cfg, &serial);
if(serial.name == NULL) {
ERR("Could not load serial configuration\n");
exit(2);
}
if(!strlen(serial.name)) {
ERR("You must specify the serial port\n");
exit(2);
}
DBG("Setting up serial port...");
state.arduino_fd = setupSerial(serial.name, serial.speed);
if(state.arduino_fd < 0) {
ERR("Failed to setup serial port %s @ %d\n", serial.name, serial.speed);
exit(2);
}
INFO("listening for event on %s\n", serial.name);
mosquitto_lib_init();
mqttserver_t mqtt;
loadMQTT(&cfg, &mqtt);
char hostname[BUF_MAX];
gethostname(hostname, BUF_MAX);
asprintf(&state.client_id, "%s.%s", hostname, (char *) strlaststr(serial.name, "/"));
mosq = mosquitto_new(state.client_id, true, &state.arduino_fd); //use port name as client id
if(!mosq) {
ERR("Couldn't create a new mosquitto client instance\n");
exit(3);
}
//TODO setup callbacks
mosquitto_log_callback_set(mosq, log_callback);
mosquitto_disconnect_callback_set(mosq, disconnect_callback);
mosquitto_connect_callback_set(mosq, connect_callback);
mosquitto_message_callback_set(mosq, message_callback);
INFO("Connecting to %s:%d ... ", mqtt.servername, mqtt.port);
if(mosquitto_connect(mosq, mqtt.servername, mqtt.port, mqtt.keepalive)){
ERR("\nUnable to connect to %s:%d.\n", mqtt.servername, mqtt.port);
exit(3);
}
INFO("done\n");
int mosq_fd = mosquitto_socket(mosq);
fd_set active_fd_set, read_fd_set;
/* Initialize the set of active sockets. */
FD_ZERO (&active_fd_set);
FD_SET (state.arduino_fd, &active_fd_set);
FD_SET (mosq_fd, &active_fd_set);
char buf[BUF_MAX];
bzero(buf,BUF_MAX);
int retries = 0;
//TODO setup syscall to stop process
while(1) {
/* Block until input arrives on one or more active sockets. */
read_fd_set = active_fd_set;
if (select (FD_SETSIZE, &read_fd_set, NULL, NULL, NULL) < 0) {
ERR("Error in select\n");
sleep(BACKOFF);
int r = mosquitto_reconnect(mosq);
retries++;
if(r != MOSQ_ERR_SUCCESS) {
ERR("Could not reconnect to broker: %s\n", strerror(r));
if(retries > MAX_RETRIES) {
/* Cleanup */
mosquitto_destroy(mosq);
mosquitto_lib_cleanup();
exit (EXIT_FAILURE);
}
} else {
retries = 0;
continue;
}
}
/* Service all the sockets with input pending. */
int i;
for (i = 0; i < FD_SETSIZE; ++i)
if (FD_ISSET (i, &read_fd_set)) {
if(i == state.arduino_fd) {
if(!readSerial(state.arduino_fd, buf, BUF_MAX, serial.timeout)) {
arduinoEvent(buf, &cfg, mosq);
bzero(buf,BUF_MAX);
}
} else if(i == mosq_fd) {
mosquitto_loop_read(mosq, 1);
mosquitto_loop_write(mosq, 1);
mosquitto_loop_misc(mosq);
}
}
};
return EXIT_SUCCESS;
}
