Diff: main.cpp

Revision:

0:42d76776f570

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Nov 28 15:14:50 2013 +0000
@@ -0,0 +1,106 @@
+#include "mbed.h"
+#include "IMU.h"
+#include "Servo.h"
+#include "PID.h"
+#include "CAN.h"
+
+#define SERVO_DEG 10.0      /* The range in degrees from centre to min or max */ 
+#define SERVO_RAN 0.005     /* The pulsewidth range from centre to min or max */
+
+#define DEF_WANT_HEIGHT 200
+#define MIN_HEIGHT 50
+#define MAX_HEIGHT 1000
+
+#define DEF_PID_KC 1
+#define DEF_PID_TAUL 1
+#define DEF_PID_TAUD 1
+#define DEF_PID_RATE 0.01
+
+#define CAN_IN_ID 0x00000384    // Only channel 900 and 901 will be received   (0000 0000 0000 0000 0000 0011 1000 0100)
+#define CAN_MASK 0xFFFFFFFF    // Compare all the bits                         (1111 1111 1111 1111 1111 1111 1111 1110) 
+
+#define CAN_BAUD 500000
+
+IMU     epson(p9,p10);
+Servo   actuator(p21);
+CAN     can(p30,p29);
+Timer   t;
+PID     pid(DEF_PID_KC,DEF_PID_TAUL,DEF_PID_TAUD,DEF_PID_RATE);
+
+float   wantedHeight = DEF_WANT_HEIGHT;
+float   kc = DEF_PID_KC;
+float   tauL = DEF_PID_TAUL;
+float   tauD = DEF_PID_TAUD;
+float   rate = DEF_PID_RATE;
+
+int     canFilterHandle = 0;
+
+/**
+    The received messages must have the following formats:
+    Can id: 900
+        Byte 0-3: Rate
+        Byte 4-7: Kc        
+        
+    Can id: 901
+        Byte 0-1: wantedHeight
+    
+*/
+CANMessage rCanMsg;
+
+void canMsgRes(void){
+    can.read(rCanMsg);     // WTF?? Does this put the message in rCanMsg??
+    
+    switch (rCanMsg.id)
+    {
+        case 900: 
+            // TODO:
+            break;
+            
+        case 901:
+            // TODO:
+            break;
+    }
+}
+
+void init(){
+    // Init the PID controller
+    pid.setOutputLimits(-1*SERVO_DEG, SERVO_DEG);
+    pid.setInputLimits(MIN_HEIGHT,MAX_HEIGHT);
+    pid.setSetPoint(wantedHeight);                            
+    pid.setMode(0);                  //Manual mode
+    pid.setProcessValue(wantedHeight);
+    pid.reset();
+    
+    // Init the IMU
+    
+    
+    // Init the Actuators
+    
+    
+    // Init the CANBus
+    can.frequency(CAN_BAUD);
+    canFilterHandle = can.filter(CAN_IN_ID,CAN_MASK);
+    can.attach(&canMsgRes);
+    
+    // Init the timer
+    t.reset();
+    
+}
+
+
+int main() {
+    // Init all the components
+    init();
+    
+    // The running loop:
+    t.start();
+    while(1){
+        // Read all the sensor data
+        
+        
+        //Wait until the timer runs out
+        wait(rate-t.read());
+        t.reset();
+    }
+    
+}
\ No newline at end of file