Gijs Bruining / Test2

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   Epson_IMU PID Servo mbed

main.cpp@0:42d76776f570, 2013-11-28 (annotated)

Committer:
GijsB
Date:
Thu Nov 28 15:14:50 2013 +0000
Revision:
0:42d76776f570
test;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
GijsB 0:42d76776f570 1 #include "mbed.h"
GijsB 0:42d76776f570 2 #include "IMU.h"
GijsB 0:42d76776f570 3 #include "Servo.h"
GijsB 0:42d76776f570 4 #include "PID.h"
GijsB 0:42d76776f570 5 #include "CAN.h"
GijsB 0:42d76776f570 6
GijsB 0:42d76776f570 7 #define SERVO_DEG 10.0 /* The range in degrees from centre to min or max */
GijsB 0:42d76776f570 8 #define SERVO_RAN 0.005 /* The pulsewidth range from centre to min or max */
GijsB 0:42d76776f570 9
GijsB 0:42d76776f570 10 #define DEF_WANT_HEIGHT 200
GijsB 0:42d76776f570 11 #define MIN_HEIGHT 50
GijsB 0:42d76776f570 12 #define MAX_HEIGHT 1000
GijsB 0:42d76776f570 13
GijsB 0:42d76776f570 14 #define DEF_PID_KC 1
GijsB 0:42d76776f570 15 #define DEF_PID_TAUL 1
GijsB 0:42d76776f570 16 #define DEF_PID_TAUD 1
GijsB 0:42d76776f570 17 #define DEF_PID_RATE 0.01
GijsB 0:42d76776f570 18
GijsB 0:42d76776f570 19 #define CAN_IN_ID 0x00000384 // Only channel 900 and 901 will be received (0000 0000 0000 0000 0000 0011 1000 0100)
GijsB 0:42d76776f570 20 #define CAN_MASK 0xFFFFFFFF // Compare all the bits (1111 1111 1111 1111 1111 1111 1111 1110)
GijsB 0:42d76776f570 21
GijsB 0:42d76776f570 22 #define CAN_BAUD 500000
GijsB 0:42d76776f570 23
GijsB 0:42d76776f570 24 IMU epson(p9,p10);
GijsB 0:42d76776f570 25 Servo actuator(p21);
GijsB 0:42d76776f570 26 CAN can(p30,p29);
GijsB 0:42d76776f570 27 Timer t;
GijsB 0:42d76776f570 28 PID pid(DEF_PID_KC,DEF_PID_TAUL,DEF_PID_TAUD,DEF_PID_RATE);
GijsB 0:42d76776f570 29
GijsB 0:42d76776f570 30 float wantedHeight = DEF_WANT_HEIGHT;
GijsB 0:42d76776f570 31 float kc = DEF_PID_KC;
GijsB 0:42d76776f570 32 float tauL = DEF_PID_TAUL;
GijsB 0:42d76776f570 33 float tauD = DEF_PID_TAUD;
GijsB 0:42d76776f570 34 float rate = DEF_PID_RATE;
GijsB 0:42d76776f570 35
GijsB 0:42d76776f570 36 int canFilterHandle = 0;
GijsB 0:42d76776f570 37
GijsB 0:42d76776f570 38 /**
GijsB 0:42d76776f570 39 The received messages must have the following formats:
GijsB 0:42d76776f570 40 Can id: 900
GijsB 0:42d76776f570 41 Byte 0-3: Rate
GijsB 0:42d76776f570 42 Byte 4-7: Kc
GijsB 0:42d76776f570 43
GijsB 0:42d76776f570 44 Can id: 901
GijsB 0:42d76776f570 45 Byte 0-1: wantedHeight
GijsB 0:42d76776f570 46
GijsB 0:42d76776f570 47 */
GijsB 0:42d76776f570 48 CANMessage rCanMsg;
GijsB 0:42d76776f570 49
GijsB 0:42d76776f570 50 void canMsgRes(void){
GijsB 0:42d76776f570 51 can.read(rCanMsg); // WTF?? Does this put the message in rCanMsg??
GijsB 0:42d76776f570 52
GijsB 0:42d76776f570 53 switch (rCanMsg.id)
GijsB 0:42d76776f570 54 {
GijsB 0:42d76776f570 55 case 900:
GijsB 0:42d76776f570 56 // TODO:
GijsB 0:42d76776f570 57 break;
GijsB 0:42d76776f570 58
GijsB 0:42d76776f570 59 case 901:
GijsB 0:42d76776f570 60 // TODO:
GijsB 0:42d76776f570 61 break;
GijsB 0:42d76776f570 62 }
GijsB 0:42d76776f570 63 }
GijsB 0:42d76776f570 64
GijsB 0:42d76776f570 65 void init(){
GijsB 0:42d76776f570 66 // Init the PID controller
GijsB 0:42d76776f570 67 pid.setOutputLimits(-1*SERVO_DEG, SERVO_DEG);
GijsB 0:42d76776f570 68 pid.setInputLimits(MIN_HEIGHT,MAX_HEIGHT);
GijsB 0:42d76776f570 69 pid.setSetPoint(wantedHeight);
GijsB 0:42d76776f570 70 pid.setMode(0); //Manual mode
GijsB 0:42d76776f570 71 pid.setProcessValue(wantedHeight);
GijsB 0:42d76776f570 72 pid.reset();
GijsB 0:42d76776f570 73
GijsB 0:42d76776f570 74 // Init the IMU
GijsB 0:42d76776f570 75
GijsB 0:42d76776f570 76
GijsB 0:42d76776f570 77 // Init the Actuators
GijsB 0:42d76776f570 78
GijsB 0:42d76776f570 79
GijsB 0:42d76776f570 80 // Init the CANBus
GijsB 0:42d76776f570 81 can.frequency(CAN_BAUD);
GijsB 0:42d76776f570 82 canFilterHandle = can.filter(CAN_IN_ID,CAN_MASK);
GijsB 0:42d76776f570 83 can.attach(&canMsgRes);
GijsB 0:42d76776f570 84
GijsB 0:42d76776f570 85 // Init the timer
GijsB 0:42d76776f570 86 t.reset();
GijsB 0:42d76776f570 87
GijsB 0:42d76776f570 88 }
GijsB 0:42d76776f570 89
GijsB 0:42d76776f570 90
GijsB 0:42d76776f570 91 int main() {
GijsB 0:42d76776f570 92 // Init all the components
GijsB 0:42d76776f570 93 init();
GijsB 0:42d76776f570 94
GijsB 0:42d76776f570 95 // The running loop:
GijsB 0:42d76776f570 96 t.start();
GijsB 0:42d76776f570 97 while(1){
GijsB 0:42d76776f570 98 // Read all the sensor data
GijsB 0:42d76776f570 99
GijsB 0:42d76776f570 100
GijsB 0:42d76776f570 101 //Wait until the timer runs out
GijsB 0:42d76776f570 102 wait(rate-t.read());
GijsB 0:42d76776f570 103 t.reset();
GijsB 0:42d76776f570 104 }
GijsB 0:42d76776f570 105
GijsB 0:42d76776f570 106 }