main.cpp

00001 #include "mbed.h"
00002 #include "math.h"
00003 
00004 #include "PreferenceWriter.h"
00005 #include "FlashWriter.h"
00006 #include "CommandProcessor.h"
00007 #include "config.h"
00008 
00009 Serial pc(USBTX, USBRX);
00010 PreferenceWriter pref(6);
00011 PwmOut led(LED1);
00012 
00013 int index = 0, loop_counter = 0;
00014 char linebuf[128];
00015 float p = 0.0f;
00016 float torque_pct = 0.0f, user_cmd = 0.0f;
00017 float vout = 0.0f;
00018 
00019 void rxCallback() {
00020     while (pc.readable()) {
00021         char c = pc.getc();
00022         if (c != 127 && c != '\r' && c != '\t') {
00023             linebuf[index] = c;
00024             index++;
00025             pc.putc(c);
00026         } else if (c == 127) {
00027             if (index > 0) {
00028                 index--;
00029                 pc.putc(c);
00030             }
00031         } else if (c == '\r') {
00032             linebuf[index] = 0;
00033             pc.putc(c);
00034             processCmd(&pc, &pref, linebuf);
00035             index = 0;
00036             pc.putc('>');
00037         }
00038     }
00039 }
00040 
00041 void slow_loop() {
00042     switch (BREMS_src) {
00043     case CMD_SRC_RC:
00044         //rc throttle code here;
00045         break;        
00046     case CMD_SRC_ANALOG:
00047         //analog throttle code here;
00048         break;
00049     case CMD_SRC_SERIAL:
00050     case CMD_SRC_TERMINAL:
00051     case CMD_SRC_CAN:
00052         //we presume these are set by free-running interrupts;
00053         break;
00054     default:
00055         break;
00056     }
00057 }
00058 
00059 void commutate() {
00060     p += 0.001f;
00061     if (p >= 6.28318f) p = 0.0f;
00062     
00063     loop_counter++;
00064     if (loop_counter % 50 == 0) {
00065         loop_counter = 0;
00066         slow_loop();
00067     }
00068     
00069     switch (BREMS_op) {
00070     case OP_TORQUE:
00071         torque_pct = user_cmd;
00072         break;
00073     case OP_DRIVING:
00074         //driving mode here;
00075         break;
00076     case OP_SPEED:
00077         //speed loop here;
00078         break;
00079     case OP_POSITION:
00080         //position loop here;
00081         break;
00082     }
00083     
00084     switch (BREMS_mode) {
00085     case MODE_RUN:
00086         vout = torque_pct * sinf(p);
00087         break;
00088     case MODE_CFG:
00089         vout = 0.0f;
00090         break;
00091     case MODE_ZERO:
00092         //zero'ing code here;
00093         break;
00094     case MODE_CHR:
00095         //wizarding code here;
00096         break;
00097     default:
00098         break;
00099     }
00100     
00101     led = 0.5f + 0.5f * vout;
00102 }
00103 
00104 int main() {    
00105     pc.baud(115200);
00106     pc.attach(rxCallback);
00107     pc.printf("%s\n", "FOC'ed in the bot version A");
00108     cmd_reload(&pc, &pref);
00109     pc.printf("%s", ">");
00110     
00111     Ticker tick;
00112     tick.attach_us(commutate, 200);
00113     
00114     for(;;) {
00115     }
00116 }