main.cpp

00001 #include "mbed.h"
00002 
00003 #include "enc.h"
00004 #include "protocol.h"
00005 #include "packet_parser.h"
00006 
00007 #define LED1 PTC2
00008 #define LED2 PTC3
00009 #define LED3 PTA18
00010 
00011 #define SERIAL_BAUDRATE 230400
00012 #define UART_TX PTD7
00013 #define UART_RX PTD6
00014 
00015 //Ticker RTI;
00016 //Serial imProc(UART_TX,UART_RX);
00017 DigitalOut led_1(LED1);
00018 //DigitalOut led_2(LED2);
00019 //DigitalOut led_3(LED3);
00020 enc motPos(PTC6,PTC7,PTC5,PTC4);
00021 PwmOut EN(PTA4);
00022 DigitalOut DR(PTA2);
00023 DigitalOut BRAKE(PTA1);
00024 AnalogIn motCurrent(PTB0);
00025 AnalogIn temp(PTE30);
00026 AnalogIn voltage(PTD5);
00027 
00028 void get_state(){
00029   motPos.update_pos();
00030 }
00031 
00032 void fill_sensor_packet(packet_t* pkt, int32_t position, int32_t velocity, uint16_t current, uint16_t voltage, uint16_t temperature) {
00033   pkt->header.type = PKT_TYPE_SENSOR;
00034   pkt->header.length = sizeof(header_t) + sizeof(sensor_data_t) + 1;
00035   sensor_data_t* sensor_data = (sensor_data_t*)pkt->data_crc;
00036   sensor_data->position = position;
00037   sensor_data->velocity = velocity;
00038   sensor_data->current = current;
00039   sensor_data->voltage = voltage;
00040   sensor_data->temperature = temperature;
00041 }
00042 
00043 int main() {  
00044   EN.period_us(50);
00045   EN.write(0.9f);
00046   DR.write(0);
00047   BRAKE.write(1);
00048   motPos.set_offset(2160);
00049   led_1 = 1;
00050 //  RTI.attach(&get_state, 0.01f);
00051 
00052   
00053   Timer system_timer;
00054   system_timer.start();
00055   uint32_t last_time = system_timer.read_ms();
00056   uint32_t current_time = last_time;
00057   
00058   
00059   PacketParser parser(SERIAL_BAUDRATE, USBTX, USBRX, LED2, LED3);
00060   packet_union_t* recv_pkt = NULL;
00061   command_data_t* command;
00062   
00063   packet_union_t* sensor_pkt = parser.get_send_packet();
00064   
00065   while(1) {
00066     recv_pkt = parser.get_received_packet();
00067 
00068     if (recv_pkt != NULL) {
00069       
00070       switch (recv_pkt->packet.header.type) {
00071         
00072         case PKT_TYPE_COMMAND:
00073           command = (command_data_t*)recv_pkt->packet.data_crc;
00074           if (sensor_pkt != NULL) {
00075             fill_sensor_packet(&(sensor_pkt->packet),
00076               motPos.ams_read(),
00077               0,
00078               motCurrent.read_u16(),
00079               voltage.read_u16(),
00080               temp.read_u16()
00081             );
00082             parser.send_packet(sensor_pkt);
00083             sensor_pkt = parser.get_send_packet();
00084           }
00085           break;
00086       }
00087       
00088       parser.free_received_packet(recv_pkt);
00089     }
00090     
00091     current_time = system_timer.read_ms();
00092     if (current_time - last_time > 500) {
00093       last_time = current_time;
00094       led_1 = !led_1;
00095     }
00096   
00097     Thread::yield();
00098   }
00099 }