wetrsf
Dependencies: Epson_IMU PID Servo mbed
main.cpp
- Committer:
- GijsB
- Date:
- 2013-11-28
- Revision:
- 1:bce26ea5279f
- Parent:
- 0:42d76776f570
File content as of revision 1:bce26ea5279f:
#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(); } }