main.cpp

00001 #include "mbed.h"
00002  
00003 #include "net.hpp"
00004  
00005 /*
00006 Ticker ticker;
00007 DigitalOut led1(LED1);
00008 DigitalOut led2(LED2);
00009 CAN can1(p9, p10);
00010 CAN can2(p30, p29);
00011 char counter = 0;
00012 
00013 void send() {
00014     printf("send()\r\n");
00015     if(can1.write(CANMessage(1337, &counter, 1))) {
00016         printf("wloop()\r\n");
00017         counter++;
00018         printf("Message sent: %d\r\n", counter);
00019     } 
00020     led1 = !led1;
00021 }
00022 */
00023 
00024 
00025 Ticker ticker;
00026 
00027 DigitalOut led_body(LED1);
00028 DigitalOut led_engine(LED2);
00029 DigitalOut led_time(LED3);
00030 DigitalOut led_driver(LED4);
00031 
00032 CAN can(p9, p10);
00033 
00034 can_cmd_body_t can_cmd_body;
00035 can_cmd_engine_t can_cmd_engine;
00036 can_cmd_driver_t can_cmd_driver;
00037 can_cmd_time_t can_cmd_time;
00038 
00039 int counter = 0;
00040 int echo = 0;
00041 
00042 #define BODY_       4
00043 #define ENGINE_     2
00044 #define DRIVER_     5
00045 #define TIME_       11
00046 
00047 void send() {
00048     //printf("send()\r\n");
00049 
00050     static int body_n_sent = 0;
00051     static int engine_n_sent = 0;
00052     static int driver_n_sent = 0;
00053     static int time_n_sent = 0;
00054 
00055     counter++;
00056 
00057     if ((counter%BODY_)==0) {
00058         can_cmd_body.payload.msg.light_r = (body_n_sent&0x0001)==0x0001;
00059         can_cmd_body.payload.msg.light_l = (body_n_sent&0x0002)==0x0002;
00060         can_cmd_body.payload.msg.light_c = (body_n_sent&0x0004)==0x0004;
00061         if(can.write(CANMessage(CAN_CMD_BODY_ID, (char*)(can_cmd_body.payload.buf), 4)))
00062             led_body = !led_body;//printf("BODY OK\r\n");
00063         else
00064             printf("BODY NOT OK\r\n");
00065         
00066         body_n_sent++;
00067     }
00068 
00069     if ((counter%ENGINE_)==0) {
00070         can_cmd_engine.payload.msg.steering = engine_n_sent;
00071         can_cmd_engine.payload.msg.power = engine_n_sent+2;
00072         can_cmd_engine.payload.msg.direction = engine_n_sent%2;
00073         can_cmd_engine.payload.msg.breaking = (engine_n_sent%2)?0:1;
00074         if(can.write(CANMessage(CAN_CMD_ENGINE_ID, (char*)(can_cmd_engine.payload.buf), 4)))
00075             led_engine = !led_engine;//printf("ENGINE OK\r\n");
00076         else
00077             printf("ENGINE NOT OK\r\n");
00078         
00079         engine_n_sent++;
00080     }
00081 
00082     if ((counter%DRIVER_)==0) {
00083         can_cmd_driver.payload.msg.cmd = 0x0A0A;
00084         echo = counter;
00085         can_cmd_driver.payload.msg.data = counter;
00086         if(can.write(CANMessage(CAN_CMD_DRIVER_ID, (char*)(can_cmd_driver.payload.buf), 8)))
00087             led_driver = !led_driver;//printf("DRIVER OK\r\n");
00088         else
00089             printf("DRIVER NOT OK\r\n");
00090         
00091         driver_n_sent++;
00092     }
00093     
00094     if ((counter%TIME_)==0) {
00095         can_cmd_time.payload.msg.time = time(NULL);
00096         if(can.write(CANMessage(CAN_CMD_TIME_ID, (char*)(can_cmd_time.payload.buf), 4)))
00097             led_time = !led_time;//printf("TIME OK\r\n");
00098         else
00099             printf("TIME NOT OK\r\n");
00100 
00101         time_n_sent++;
00102     }
00103     
00104 }
00105 
00106 
00107 int main() {
00108 /*
00109     printf("main()\r\n");
00110     ticker.attach(&send, 1);
00111     CANMessage msg;
00112     while(1) {
00113         printf("loop()\r\n");
00114         if(can2.read(msg)) {
00115             printf("Message received: %d\r\n", msg.data[0]);
00116             led2 = !led2;
00117         } 
00118         wait(0.2);
00119     }
00120 */
00121 
00122     printf("main()\r\n");
00123     ticker.attach(&send, 0.5);
00124 set_time(0);
00125     CANMessage msg;
00126     while(1) {
00127         if(can.read(msg)) {
00128             switch(msg.id) {
00129             case CAN_CMD_BODY_ID:
00130                 can_sts_body_payload_t x;
00131                 x.buf[0] = msg.data[0]; x.buf[1] = msg.data[1]; x.buf[2] = msg.data[2]; x.buf[3] = msg.data[3];
00132                 x.buf[4] = msg.data[4]; x.buf[5] = msg.data[5]; x.buf[6] = msg.data[6]; x.buf[7] = msg.data[7];
00133                 printf("Message received: %d %d %d %d\r\n", x.buf[0], x.msg.eye_back_l, x.msg.eye_back_r, x.msg.eye_front);
00134                 break;
00135             case CAN_STS_DRIVER_ID:
00136                 can_sts_driver_payload_t y;
00137                 y.buf[0] = msg.data[0]; y.buf[1] = msg.data[1]; y.buf[2] = msg.data[2]; y.buf[3] = msg.data[3];
00138                 if (y.msg.data == echo)
00139                     printf("ECHO %d\r\n", y.msg.data);
00140                 else 
00141                     printf("NO ECHO %d\r\n", y.msg.data);
00142                 break;
00143             }
00144         }
00145         wait(1.0);
00146     }
00147 }