version 1.0
Dependencies: CMSIS_DSP_401 GPS MPU9150_DMP PID QuaternionMath Servo mbed
Fork of SolarOnFoils_MainModule_20150518 by
main.cpp@1:b4a0d63db637, 2015-06-26 (annotated)
- Committer:
- Dannis_mbed
- Date:
- Fri Jun 26 09:21:33 2015 +0000
- Revision:
- 1:b4a0d63db637
- Parent:
- 0:81b21910454e
- Child:
- 2:f6d058931b17
presentation version
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Dannis_mbed | 0:81b21910454e | 1 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 2 | // // |
Dannis_mbed | 0:81b21910454e | 3 | // File : main.cpp // |
Dannis_mbed | 0:81b21910454e | 4 | // Version : 0.1 // |
Dannis_mbed | 0:81b21910454e | 5 | // Date : 25 march 2015 // |
Dannis_mbed | 0:81b21910454e | 6 | // Author : Dany Brugman // |
Dannis_mbed | 0:81b21910454e | 7 | // Comment : Function to write data to a 2x16 LCD by I2C // |
Dannis_mbed | 0:81b21910454e | 8 | // using a MCP23017 port expander. // |
Dannis_mbed | 0:81b21910454e | 9 | // // |
Dannis_mbed | 0:81b21910454e | 10 | // Changelog : // |
Dannis_mbed | 0:81b21910454e | 11 | // Date: Name: Comment: // |
Dannis_mbed | 0:81b21910454e | 12 | // 25/03/2015 DNB First version // |
Dannis_mbed | 0:81b21910454e | 13 | // // |
Dannis_mbed | 0:81b21910454e | 14 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 15 | |
Dannis_mbed | 0:81b21910454e | 16 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 17 | // includes // |
Dannis_mbed | 0:81b21910454e | 18 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 19 | |
Dannis_mbed | 0:81b21910454e | 20 | #include "mbed.h" |
Dannis_mbed | 0:81b21910454e | 21 | #include <stdio.h> |
Dannis_mbed | 0:81b21910454e | 22 | #include <stdlib.h> |
Dannis_mbed | 0:81b21910454e | 23 | #include <string.h> |
Dannis_mbed | 0:81b21910454e | 24 | #include "MCP23017.h" |
Dannis_mbed | 0:81b21910454e | 25 | #include "LCD_I2C.h" |
Dannis_mbed | 0:81b21910454e | 26 | #include "uart.h" |
Dannis_mbed | 0:81b21910454e | 27 | #include "GPS.h" |
Dannis_mbed | 0:81b21910454e | 28 | #include "MPU9150.h" |
Dannis_mbed | 0:81b21910454e | 29 | #include "Quaternion.h" |
Dannis_mbed | 0:81b21910454e | 30 | #include "PID.h" |
Dannis_mbed | 0:81b21910454e | 31 | #include "Servo.h" |
Dannis_mbed | 0:81b21910454e | 32 | #include "mRotaryEncoder.h" |
Dannis_mbed | 0:81b21910454e | 33 | #include "MainModule.h" |
Dannis_mbed | 1:b4a0d63db637 | 34 | //#include "LCD_Menu.h" |
Dannis_mbed | 0:81b21910454e | 35 | #include "menu.h" |
Dannis_mbed | 0:81b21910454e | 36 | #include "systemVar.h" |
Dannis_mbed | 0:81b21910454e | 37 | |
Dannis_mbed | 0:81b21910454e | 38 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 39 | // defines // |
Dannis_mbed | 0:81b21910454e | 40 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 41 | |
Dannis_mbed | 0:81b21910454e | 42 | #define CLEAR "\033[2J" |
Dannis_mbed | 0:81b21910454e | 43 | #define PROJECT "\033[1;10fSolar on Foils\n" |
Dannis_mbed | 0:81b21910454e | 44 | #define VERSION "\033[2;10fVersion 1.0\n" |
Dannis_mbed | 0:81b21910454e | 45 | |
Dannis_mbed | 1:b4a0d63db637 | 46 | #define EXT_UI_ID 20 |
Dannis_mbed | 0:81b21910454e | 47 | #define ACTUATOR_PORT_ID 101 |
Dannis_mbed | 0:81b21910454e | 48 | #define ACTUATOR_STARB_ID 102 |
Dannis_mbed | 1:b4a0d63db637 | 49 | #define ACTUATOR_PORT_RUN 103 |
Dannis_mbed | 1:b4a0d63db637 | 50 | #define ACTUATOR_STARB_RUN 104 |
Dannis_mbed | 0:81b21910454e | 51 | #define HEIGHT_PORT_ID 201 |
Dannis_mbed | 0:81b21910454e | 52 | #define HEIGHT_STARB_ID 202 |
Dannis_mbed | 1:b4a0d63db637 | 53 | #define HEIGHT_PORT_DATA 203 |
Dannis_mbed | 1:b4a0d63db637 | 54 | #define HEIGHT_STARB_DATA 204 |
Dannis_mbed | 0:81b21910454e | 55 | #define GYRO_GPS_ID 205 |
Dannis_mbed | 1:b4a0d63db637 | 56 | #define EXT_UI_HEIGHT 1020 |
Dannis_mbed | 0:81b21910454e | 57 | #define MESSAGE_ALL 2000 |
Dannis_mbed | 0:81b21910454e | 58 | #define PORT_ACT_DIAGN 2001 |
Dannis_mbed | 0:81b21910454e | 59 | #define STARB_ACT_DIAGN 2002 |
Dannis_mbed | 0:81b21910454e | 60 | #define PORT_HGHT_DIAGN 2003 |
Dannis_mbed | 0:81b21910454e | 61 | #define STARB_HGHT_DIAGN 2004 |
Dannis_mbed | 0:81b21910454e | 62 | #define GYRO_GPS_DIAGN 2005 |
Dannis_mbed | 0:81b21910454e | 63 | #define EXT_UI_DIAGN 2006 |
Dannis_mbed | 0:81b21910454e | 64 | |
Dannis_mbed | 0:81b21910454e | 65 | #define CCW 0 |
Dannis_mbed | 0:81b21910454e | 66 | #define CW 1 |
Dannis_mbed | 0:81b21910454e | 67 | |
Dannis_mbed | 0:81b21910454e | 68 | #define V_MAX 1.85 |
Dannis_mbed | 0:81b21910454e | 69 | #define NSAMPLES 10000 //current samples for ref. |
Dannis_mbed | 0:81b21910454e | 70 | |
Dannis_mbed | 0:81b21910454e | 71 | #define ROLL_IN_MIN -90 |
Dannis_mbed | 0:81b21910454e | 72 | #define ROLL_IN_MAX 90 |
Dannis_mbed | 0:81b21910454e | 73 | #define ROLL_OUT_MIN -0.1 |
Dannis_mbed | 0:81b21910454e | 74 | #define ROLL_OUT_MAX 0.1 |
Dannis_mbed | 0:81b21910454e | 75 | |
Dannis_mbed | 0:81b21910454e | 76 | #define Kc 1.0 |
Dannis_mbed | 0:81b21910454e | 77 | #define Ti 0.0 |
Dannis_mbed | 0:81b21910454e | 78 | #define Td 0.0 |
Dannis_mbed | 0:81b21910454e | 79 | #define RATE 0.01 |
Dannis_mbed | 0:81b21910454e | 80 | |
Dannis_mbed | 0:81b21910454e | 81 | enum MSG_t |
Dannis_mbed | 0:81b21910454e | 82 | { |
Dannis_mbed | 0:81b21910454e | 83 | MSG1 = 0, // message 1 |
Dannis_mbed | 0:81b21910454e | 84 | MSG2, // message 2 |
Dannis_mbed | 0:81b21910454e | 85 | MSG3 // message 3 |
Dannis_mbed | 0:81b21910454e | 86 | }; |
Dannis_mbed | 0:81b21910454e | 87 | |
Dannis_mbed | 0:81b21910454e | 88 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 89 | // port declaration // |
Dannis_mbed | 0:81b21910454e | 90 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 91 | |
Dannis_mbed | 0:81b21910454e | 92 | DigitalOut myLed(LED1); |
Dannis_mbed | 0:81b21910454e | 93 | DigitalOut myLed2(LED2); |
Dannis_mbed | 0:81b21910454e | 94 | |
Dannis_mbed | 0:81b21910454e | 95 | DigitalOut aliveLed(p23); |
Dannis_mbed | 0:81b21910454e | 96 | DigitalOut statusLed(p26); |
Dannis_mbed | 0:81b21910454e | 97 | DigitalOut emergencyLed(p25); |
Dannis_mbed | 0:81b21910454e | 98 | DigitalOut motorRelais(p20); |
Dannis_mbed | 0:81b21910454e | 99 | |
Dannis_mbed | 0:81b21910454e | 100 | AnalogIn currentSensor(p15); |
Dannis_mbed | 0:81b21910454e | 101 | |
Dannis_mbed | 0:81b21910454e | 102 | InterruptIn intAGM(p8); |
Dannis_mbed | 0:81b21910454e | 103 | |
Dannis_mbed | 0:81b21910454e | 104 | I2C i2c(p9, p10); |
Dannis_mbed | 0:81b21910454e | 105 | GPS gps(p13, p14); |
Dannis_mbed | 0:81b21910454e | 106 | // I2C Rx, I2C Tx, Int |
Dannis_mbed | 0:81b21910454e | 107 | MPU9150 imu(p10, p9, p8); |
Dannis_mbed | 0:81b21910454e | 108 | //mRotaryEncoder(PinName pinA, PinName pinB, PinName pinSW, PinMode pullMode=PullUp, int debounceTime_us=1000) |
Dannis_mbed | 0:81b21910454e | 109 | mRotaryEncoder rSwitch(p12, p11, p7); |
Dannis_mbed | 0:81b21910454e | 110 | CAN CANbus(p30, p29); |
Dannis_mbed | 0:81b21910454e | 111 | MCP23017 mcp23017 = MCP23017(i2c, 0x40); |
Dannis_mbed | 0:81b21910454e | 112 | Serial debug(USBTX, USBRX); |
Dannis_mbed | 0:81b21910454e | 113 | |
Dannis_mbed | 0:81b21910454e | 114 | PID rollPID(Kc,Ti,Td,RATE); // declare a PID for roll adjustment |
Dannis_mbed | 0:81b21910454e | 115 | |
Dannis_mbed | 0:81b21910454e | 116 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 117 | // function prototypes // |
Dannis_mbed | 0:81b21910454e | 118 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 119 | |
Dannis_mbed | 0:81b21910454e | 120 | |
Dannis_mbed | 0:81b21910454e | 121 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 122 | // global variables // |
Dannis_mbed | 0:81b21910454e | 123 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 124 | |
Dannis_mbed | 0:81b21910454e | 125 | int i = 0; |
Dannis_mbed | 0:81b21910454e | 126 | int value = 0; |
Dannis_mbed | 0:81b21910454e | 127 | int lastGet; |
Dannis_mbed | 0:81b21910454e | 128 | int thisGet; |
Dannis_mbed | 0:81b21910454e | 129 | int iFilter; |
Dannis_mbed | 0:81b21910454e | 130 | uint32_t uiFilterId; |
Dannis_mbed | 1:b4a0d63db637 | 131 | |
Dannis_mbed | 0:81b21910454e | 132 | |
Dannis_mbed | 0:81b21910454e | 133 | bool enc_pressed = false; // Button of rotaryencoder was pressed |
Dannis_mbed | 0:81b21910454e | 134 | bool enc_rotated = false; // rotary encoder was totaded left or right |
Dannis_mbed | 0:81b21910454e | 135 | bool bSystemFail = false; // sytem fail |
Dannis_mbed | 0:81b21910454e | 136 | |
Dannis_mbed | 0:81b21910454e | 137 | char buffer_GPIOB; |
Dannis_mbed | 0:81b21910454e | 138 | char cBuffer[200]; |
Dannis_mbed | 0:81b21910454e | 139 | char cMessage; |
Dannis_mbed | 0:81b21910454e | 140 | char text[16]; |
Dannis_mbed | 1:b4a0d63db637 | 141 | char cPTurns[2] ={0}; |
Dannis_mbed | 1:b4a0d63db637 | 142 | char cSTurns[2] ={0}; |
Dannis_mbed | 0:81b21910454e | 143 | //char buffer[16]; |
Dannis_mbed | 0:81b21910454e | 144 | |
Dannis_mbed | 0:81b21910454e | 145 | float fRoll, fPitch, fEulerRoll; |
Dannis_mbed | 0:81b21910454e | 146 | float fRollDiff; |
Dannis_mbed | 0:81b21910454e | 147 | float fPreviousTime; |
Dannis_mbed | 0:81b21910454e | 148 | |
Dannis_mbed | 0:81b21910454e | 149 | double dCurrent = 0.0; |
Dannis_mbed | 0:81b21910454e | 150 | double dRefCurrent = 0.0; |
Dannis_mbed | 0:81b21910454e | 151 | |
Dannis_mbed | 0:81b21910454e | 152 | Quaternion q1; |
Dannis_mbed | 0:81b21910454e | 153 | |
Dannis_mbed | 0:81b21910454e | 154 | Ticker CANTicker; |
Dannis_mbed | 0:81b21910454e | 155 | Ticker tAliveOn; |
Dannis_mbed | 0:81b21910454e | 156 | Ticker tEmergencyLedOn; |
Dannis_mbed | 0:81b21910454e | 157 | Ticker tMPU; |
Dannis_mbed | 0:81b21910454e | 158 | |
Dannis_mbed | 0:81b21910454e | 159 | Timeout tAliveOff; |
Dannis_mbed | 0:81b21910454e | 160 | Timeout tEmergencyLedOff; |
Dannis_mbed | 0:81b21910454e | 161 | Timer actualTime; |
Dannis_mbed | 0:81b21910454e | 162 | Timer timer; |
Dannis_mbed | 0:81b21910454e | 163 | |
Dannis_mbed | 0:81b21910454e | 164 | Menu mLCDMenu; |
Dannis_mbed | 0:81b21910454e | 165 | SystemVar svSoF; |
Dannis_mbed | 0:81b21910454e | 166 | |
Dannis_mbed | 0:81b21910454e | 167 | MSG_t messageToSend; |
Dannis_mbed | 0:81b21910454e | 168 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 169 | // main // |
Dannis_mbed | 0:81b21910454e | 170 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 171 | |
Dannis_mbed | 0:81b21910454e | 172 | int main() |
Dannis_mbed | 0:81b21910454e | 173 | { |
Dannis_mbed | 0:81b21910454e | 174 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 175 | // init // |
Dannis_mbed | 0:81b21910454e | 176 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 177 | vInit(); |
Dannis_mbed | 0:81b21910454e | 178 | // init CANbus |
Dannis_mbed | 0:81b21910454e | 179 | vInitCANBus(); |
Dannis_mbed | 0:81b21910454e | 180 | // init Gyro |
Dannis_mbed | 0:81b21910454e | 181 | vInitImu(); |
Dannis_mbed | 0:81b21910454e | 182 | |
Dannis_mbed | 0:81b21910454e | 183 | |
Dannis_mbed | 0:81b21910454e | 184 | CANMessage msg; |
Dannis_mbed | 0:81b21910454e | 185 | messageToSend = MSG1; |
Dannis_mbed | 0:81b21910454e | 186 | |
Dannis_mbed | 0:81b21910454e | 187 | |
Dannis_mbed | 0:81b21910454e | 188 | //acquiring v_ref |
Dannis_mbed | 0:81b21910454e | 189 | for (int i=0; i<NSAMPLES; i++) |
Dannis_mbed | 0:81b21910454e | 190 | { |
Dannis_mbed | 0:81b21910454e | 191 | dRefCurrent += currentSensor; |
Dannis_mbed | 0:81b21910454e | 192 | } |
Dannis_mbed | 0:81b21910454e | 193 | dRefCurrent /= NSAMPLES; |
Dannis_mbed | 0:81b21910454e | 194 | //debug.printf("RefCurrent : %f\n\r", dRefCurrent); |
Dannis_mbed | 0:81b21910454e | 195 | |
Dannis_mbed | 0:81b21910454e | 196 | timer.start(); //timer to test imu roll angle |
Dannis_mbed | 0:81b21910454e | 197 | actualTime.start(); //crash timer |
Dannis_mbed | 0:81b21910454e | 198 | // clear line -1- |
Dannis_mbed | 0:81b21910454e | 199 | vLCD_printPos_I2C((unsigned char*)" ", 1, 1); |
Dannis_mbed | 0:81b21910454e | 200 | |
Dannis_mbed | 0:81b21910454e | 201 | // put text on lcd |
Dannis_mbed | 0:81b21910454e | 202 | |
Dannis_mbed | 1:b4a0d63db637 | 203 | //mLCDMenu.vShowScreen(_FOILBORNE); |
Dannis_mbed | 0:81b21910454e | 204 | //vLCD_printPos_I2C((unsigned char*)mLCDMenu.getLine1(), 2, 1); |
Dannis_mbed | 0:81b21910454e | 205 | |
Dannis_mbed | 0:81b21910454e | 206 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 207 | // endless loop // |
Dannis_mbed | 0:81b21910454e | 208 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 209 | while (1) |
Dannis_mbed | 0:81b21910454e | 210 | { |
Dannis_mbed | 0:81b21910454e | 211 | // rotary switch has been rotated? |
Dannis_mbed | 0:81b21910454e | 212 | if (enc_rotated) vSwitchRotated(); |
Dannis_mbed | 0:81b21910454e | 213 | |
Dannis_mbed | 0:81b21910454e | 214 | // rotary switch confirm pressed? |
Dannis_mbed | 0:81b21910454e | 215 | if (enc_pressed) vSwitchConfirmed(); |
Dannis_mbed | 0:81b21910454e | 216 | |
Dannis_mbed | 1:b4a0d63db637 | 217 | // answer CAN |
Dannis_mbed | 1:b4a0d63db637 | 218 | if(CANbus.read(msg)){ |
Dannis_mbed | 1:b4a0d63db637 | 219 | if (msg.id == EXT_UI_HEIGHT) svSoF.vVarHeightFoilBorne((uint32_t) msg.data[0]); |
Dannis_mbed | 1:b4a0d63db637 | 220 | } |
Dannis_mbed | 0:81b21910454e | 221 | if(CANbus.read(msg)){ |
Dannis_mbed | 1:b4a0d63db637 | 222 | debug.printf("ID: %i.\t", msg.id); |
Dannis_mbed | 1:b4a0d63db637 | 223 | if ((msg.id == HEIGHT_PORT_DATA)&&(mLCDMenu.bGetReadHeight() == true)) |
Dannis_mbed | 1:b4a0d63db637 | 224 | { |
Dannis_mbed | 1:b4a0d63db637 | 225 | svSoF.vSetPHeight((uint32_t) msg.data[0]); |
Dannis_mbed | 1:b4a0d63db637 | 226 | debug.printf("height received: %c cm\r\n", msg.data[0]); |
Dannis_mbed | 1:b4a0d63db637 | 227 | } |
Dannis_mbed | 1:b4a0d63db637 | 228 | //if ((msg.id == ACTUATOR_PORT_ID)&&(msg.data[0] == 0xFF)) statusLed = ! statusLed; |
Dannis_mbed | 0:81b21910454e | 229 | //debug.printf("ID#102 received: %d answer.\r\n", msg.data[0]); |
Dannis_mbed | 0:81b21910454e | 230 | } |
Dannis_mbed | 0:81b21910454e | 231 | |
Dannis_mbed | 1:b4a0d63db637 | 232 | if(timer.read_ms() > 250) |
Dannis_mbed | 0:81b21910454e | 233 | { |
Dannis_mbed | 0:81b21910454e | 234 | timer.reset(); |
Dannis_mbed | 1:b4a0d63db637 | 235 | if(svSoF.uiGetRoll() >= 4) |
Dannis_mbed | 1:b4a0d63db637 | 236 | { |
Dannis_mbed | 1:b4a0d63db637 | 237 | if(svSoF.iGetRollPolarity() == 0) |
Dannis_mbed | 1:b4a0d63db637 | 238 | { |
Dannis_mbed | 1:b4a0d63db637 | 239 | cPTurns[1] = 1; |
Dannis_mbed | 1:b4a0d63db637 | 240 | cSTurns[1] = 0; |
Dannis_mbed | 1:b4a0d63db637 | 241 | } |
Dannis_mbed | 1:b4a0d63db637 | 242 | else |
Dannis_mbed | 1:b4a0d63db637 | 243 | { |
Dannis_mbed | 1:b4a0d63db637 | 244 | cPTurns[1] = 0; |
Dannis_mbed | 1:b4a0d63db637 | 245 | cSTurns[1] = 1; |
Dannis_mbed | 1:b4a0d63db637 | 246 | } |
Dannis_mbed | 1:b4a0d63db637 | 247 | cPTurns[0] = 1; |
Dannis_mbed | 1:b4a0d63db637 | 248 | cSTurns[0] = 1; |
Dannis_mbed | 1:b4a0d63db637 | 249 | |
Dannis_mbed | 1:b4a0d63db637 | 250 | CANbus.write(CANMessage(ACTUATOR_PORT_RUN, cPTurns, 2)); // send message to port actuator |
Dannis_mbed | 1:b4a0d63db637 | 251 | wait(0.15); |
Dannis_mbed | 1:b4a0d63db637 | 252 | CANbus.write(CANMessage(ACTUATOR_STARB_RUN, cSTurns, 2)); // send message to port actuator |
Dannis_mbed | 1:b4a0d63db637 | 253 | |
Dannis_mbed | 1:b4a0d63db637 | 254 | }//end if |
Dannis_mbed | 0:81b21910454e | 255 | svSoF.vSetPitch(q1); // call function class SystemVar |
Dannis_mbed | 1:b4a0d63db637 | 256 | svSoF.vSetRoll(q1); |
Dannis_mbed | 1:b4a0d63db637 | 257 | mLCDMenu.vShowScreen(mLCDMenu.getScreen()); |
Dannis_mbed | 0:81b21910454e | 258 | vLCD_update(); |
Dannis_mbed | 0:81b21910454e | 259 | |
Dannis_mbed | 0:81b21910454e | 260 | //debug.printf("Roll: %c\t", svSoF.getValue()); |
Dannis_mbed | 0:81b21910454e | 261 | //debug.printf("Roll: %f\t", getRollAngle(q1)); |
Dannis_mbed | 0:81b21910454e | 262 | //debug.printf("Pitch: %f\t", getPitchAngle(q1)); |
Dannis_mbed | 0:81b21910454e | 263 | //debug.printf("Time: %f\r\n", actualTime.read()); |
Dannis_mbed | 0:81b21910454e | 264 | } |
Dannis_mbed | 0:81b21910454e | 265 | /*for (int samples = 0; samples<NSAMPLES; samples++) dCurrent += currentSensor; |
Dannis_mbed | 0:81b21910454e | 266 | |
Dannis_mbed | 0:81b21910454e | 267 | dCurrent /= NSAMPLES; |
Dannis_mbed | 0:81b21910454e | 268 | //debug.printf("RefCurrent : %f\n\r", dRefCurrent); |
Dannis_mbed | 0:81b21910454e | 269 | //debug.printf("dCurrent : %f\n\r", dCurrent); |
Dannis_mbed | 0:81b21910454e | 270 | dCurrent = (dCurrent-dRefCurrent)*V_MAX/0.185; |
Dannis_mbed | 0:81b21910454e | 271 | |
Dannis_mbed | 0:81b21910454e | 272 | debug.printf("Current mA : %f\n\r", (dCurrent*1000));*/ |
Dannis_mbed | 0:81b21910454e | 273 | wait(0.2); |
Dannis_mbed | 0:81b21910454e | 274 | |
Dannis_mbed | 0:81b21910454e | 275 | } |
Dannis_mbed | 0:81b21910454e | 276 | |
Dannis_mbed | 0:81b21910454e | 277 | } // END MAIN |
Dannis_mbed | 0:81b21910454e | 278 | |
Dannis_mbed | 0:81b21910454e | 279 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 280 | // delay function for LCD // |
Dannis_mbed | 0:81b21910454e | 281 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 282 | |
Dannis_mbed | 0:81b21910454e | 283 | void vLCD_delay_I2C (unsigned int t_delay) |
Dannis_mbed | 0:81b21910454e | 284 | { |
Dannis_mbed | 0:81b21910454e | 285 | unsigned int i; |
Dannis_mbed | 0:81b21910454e | 286 | |
Dannis_mbed | 0:81b21910454e | 287 | for (i=0; i < t_delay; i++); |
Dannis_mbed | 0:81b21910454e | 288 | } |
Dannis_mbed | 0:81b21910454e | 289 | |
Dannis_mbed | 0:81b21910454e | 290 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 291 | // write commando to LCD // |
Dannis_mbed | 0:81b21910454e | 292 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 293 | |
Dannis_mbed | 0:81b21910454e | 294 | void vLCD_cmd_I2C (unsigned char commando) |
Dannis_mbed | 0:81b21910454e | 295 | { |
Dannis_mbed | 0:81b21910454e | 296 | __disable_irq(); // Disable Interrupts |
Dannis_mbed | 0:81b21910454e | 297 | |
Dannis_mbed | 0:81b21910454e | 298 | // write commando to LCD |
Dannis_mbed | 0:81b21910454e | 299 | mcp23017.write(PORT_A, commando); |
Dannis_mbed | 0:81b21910454e | 300 | |
Dannis_mbed | 0:81b21910454e | 301 | // LCD_RS = 0 |
Dannis_mbed | 0:81b21910454e | 302 | buffer_GPIOB = buffer_GPIOB & 0xFD; // RS = 0 |
Dannis_mbed | 0:81b21910454e | 303 | // write to LCD |
Dannis_mbed | 0:81b21910454e | 304 | mcp23017.write(PORT_B, buffer_GPIOB); |
Dannis_mbed | 0:81b21910454e | 305 | |
Dannis_mbed | 0:81b21910454e | 306 | // LCD_EN = 1 |
Dannis_mbed | 0:81b21910454e | 307 | buffer_GPIOB = buffer_GPIOB | 0x01; // EN = 1 |
Dannis_mbed | 0:81b21910454e | 308 | // write to LCD |
Dannis_mbed | 0:81b21910454e | 309 | mcp23017.write(PORT_B, buffer_GPIOB); |
Dannis_mbed | 0:81b21910454e | 310 | |
Dannis_mbed | 0:81b21910454e | 311 | // LCD_EN = 0 |
Dannis_mbed | 0:81b21910454e | 312 | buffer_GPIOB = buffer_GPIOB & 0xFE; // EN = 0 |
Dannis_mbed | 0:81b21910454e | 313 | // write to LCD |
Dannis_mbed | 0:81b21910454e | 314 | mcp23017.write(PORT_B, buffer_GPIOB); |
Dannis_mbed | 0:81b21910454e | 315 | |
Dannis_mbed | 0:81b21910454e | 316 | __enable_irq(); // Enable Interrupts |
Dannis_mbed | 0:81b21910454e | 317 | } |
Dannis_mbed | 0:81b21910454e | 318 | |
Dannis_mbed | 0:81b21910454e | 319 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 320 | // write single char to LCD // |
Dannis_mbed | 0:81b21910454e | 321 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 322 | |
Dannis_mbed | 0:81b21910454e | 323 | void vLCD_data_I2C (unsigned char data) |
Dannis_mbed | 0:81b21910454e | 324 | { |
Dannis_mbed | 0:81b21910454e | 325 | __disable_irq(); // Disable Interrupts |
Dannis_mbed | 0:81b21910454e | 326 | |
Dannis_mbed | 0:81b21910454e | 327 | // write data to LCD |
Dannis_mbed | 0:81b21910454e | 328 | mcp23017.write(PORT_A, data); |
Dannis_mbed | 0:81b21910454e | 329 | |
Dannis_mbed | 0:81b21910454e | 330 | // LCD_RS = 1 |
Dannis_mbed | 0:81b21910454e | 331 | buffer_GPIOB = buffer_GPIOB | 0x02; // RS = 1 |
Dannis_mbed | 0:81b21910454e | 332 | // write to LCD |
Dannis_mbed | 0:81b21910454e | 333 | mcp23017.write(PORT_B, buffer_GPIOB); |
Dannis_mbed | 0:81b21910454e | 334 | |
Dannis_mbed | 0:81b21910454e | 335 | // LCD_EN = 1 |
Dannis_mbed | 0:81b21910454e | 336 | buffer_GPIOB = buffer_GPIOB | 0x01; // EN = 1 |
Dannis_mbed | 0:81b21910454e | 337 | // write to LCD |
Dannis_mbed | 0:81b21910454e | 338 | mcp23017.write(PORT_B, buffer_GPIOB); |
Dannis_mbed | 0:81b21910454e | 339 | |
Dannis_mbed | 0:81b21910454e | 340 | // LCD_EN = 0 |
Dannis_mbed | 0:81b21910454e | 341 | buffer_GPIOB = buffer_GPIOB & 0xFE; // EN = 0 |
Dannis_mbed | 0:81b21910454e | 342 | // write to LCD |
Dannis_mbed | 0:81b21910454e | 343 | mcp23017.write(PORT_B, buffer_GPIOB); |
Dannis_mbed | 0:81b21910454e | 344 | |
Dannis_mbed | 0:81b21910454e | 345 | __enable_irq(); // Enable Interrupts |
Dannis_mbed | 0:81b21910454e | 346 | } |
Dannis_mbed | 0:81b21910454e | 347 | |
Dannis_mbed | 0:81b21910454e | 348 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 349 | // Initialize LCD // |
Dannis_mbed | 0:81b21910454e | 350 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 351 | |
Dannis_mbed | 0:81b21910454e | 352 | void vLCD_init_I2C (void) |
Dannis_mbed | 0:81b21910454e | 353 | { |
Dannis_mbed | 0:81b21910454e | 354 | vLCD_cmd_I2C (0x00); |
Dannis_mbed | 0:81b21910454e | 355 | // vLCD_delay_I2C (65535) |
Dannis_mbed | 0:81b21910454e | 356 | vLCD_delay_I2C (100); |
Dannis_mbed | 0:81b21910454e | 357 | |
Dannis_mbed | 0:81b21910454e | 358 | vLCD_cmd_I2C (0x38); |
Dannis_mbed | 0:81b21910454e | 359 | // vLCD_delay_I2C (65535) |
Dannis_mbed | 0:81b21910454e | 360 | vLCD_delay_I2C (100); |
Dannis_mbed | 0:81b21910454e | 361 | |
Dannis_mbed | 0:81b21910454e | 362 | vLCD_cmd_I2C (0x38); |
Dannis_mbed | 0:81b21910454e | 363 | //vLCD_delay_I2C (65535) |
Dannis_mbed | 0:81b21910454e | 364 | vLCD_delay_I2C (100); |
Dannis_mbed | 0:81b21910454e | 365 | |
Dannis_mbed | 0:81b21910454e | 366 | vLCD_cmd_I2C (0x38); |
Dannis_mbed | 0:81b21910454e | 367 | //vLCD_delay_I2C (65535) |
Dannis_mbed | 0:81b21910454e | 368 | vLCD_delay_I2C (100); |
Dannis_mbed | 0:81b21910454e | 369 | |
Dannis_mbed | 0:81b21910454e | 370 | // Display ON / OFF |
Dannis_mbed | 0:81b21910454e | 371 | vLCD_cmd_I2C (0x08); |
Dannis_mbed | 0:81b21910454e | 372 | |
Dannis_mbed | 0:81b21910454e | 373 | //vLCD_delay_I2C (65535) |
Dannis_mbed | 0:81b21910454e | 374 | vLCD_delay_I2C (100); |
Dannis_mbed | 0:81b21910454e | 375 | |
Dannis_mbed | 0:81b21910454e | 376 | // Clear Display |
Dannis_mbed | 0:81b21910454e | 377 | vLCD_cmd_I2C (0x01); |
Dannis_mbed | 0:81b21910454e | 378 | |
Dannis_mbed | 0:81b21910454e | 379 | //vLCD_delay_I2C (65535) |
Dannis_mbed | 0:81b21910454e | 380 | vLCD_delay_I2C (100); |
Dannis_mbed | 0:81b21910454e | 381 | |
Dannis_mbed | 0:81b21910454e | 382 | // Entry Mode Set |
Dannis_mbed | 0:81b21910454e | 383 | vLCD_cmd_I2C (0x06); |
Dannis_mbed | 0:81b21910454e | 384 | |
Dannis_mbed | 0:81b21910454e | 385 | //vLCD_delay_I2C (65535) |
Dannis_mbed | 0:81b21910454e | 386 | vLCD_delay_I2C (100); |
Dannis_mbed | 0:81b21910454e | 387 | |
Dannis_mbed | 0:81b21910454e | 388 | // Display ON / OFF |
Dannis_mbed | 0:81b21910454e | 389 | vLCD_cmd_I2C (0x0C); |
Dannis_mbed | 0:81b21910454e | 390 | |
Dannis_mbed | 0:81b21910454e | 391 | //vLCD_delay_I2C (65535) |
Dannis_mbed | 0:81b21910454e | 392 | vLCD_delay_I2C (100); |
Dannis_mbed | 0:81b21910454e | 393 | } |
Dannis_mbed | 0:81b21910454e | 394 | |
Dannis_mbed | 0:81b21910454e | 395 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 396 | // Write string to LCD // |
Dannis_mbed | 0:81b21910454e | 397 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 398 | |
Dannis_mbed | 0:81b21910454e | 399 | void vLCD_print_I2C (unsigned char *string, unsigned char line) |
Dannis_mbed | 0:81b21910454e | 400 | { |
Dannis_mbed | 0:81b21910454e | 401 | unsigned int i; |
Dannis_mbed | 0:81b21910454e | 402 | |
Dannis_mbed | 0:81b21910454e | 403 | // select target line to write to |
Dannis_mbed | 0:81b21910454e | 404 | if (line == 1) |
Dannis_mbed | 0:81b21910454e | 405 | { |
Dannis_mbed | 0:81b21910454e | 406 | vLCD_cmd_I2C (0x80); |
Dannis_mbed | 0:81b21910454e | 407 | } |
Dannis_mbed | 0:81b21910454e | 408 | else if (line == 2) |
Dannis_mbed | 0:81b21910454e | 409 | { |
Dannis_mbed | 0:81b21910454e | 410 | vLCD_cmd_I2C (0xC0); |
Dannis_mbed | 0:81b21910454e | 411 | } |
Dannis_mbed | 0:81b21910454e | 412 | else |
Dannis_mbed | 0:81b21910454e | 413 | { |
Dannis_mbed | 0:81b21910454e | 414 | return; // End of function: Error in the past value of line |
Dannis_mbed | 0:81b21910454e | 415 | } |
Dannis_mbed | 0:81b21910454e | 416 | |
Dannis_mbed | 0:81b21910454e | 417 | // write data to selected line |
Dannis_mbed | 0:81b21910454e | 418 | for (i=0; i<16; i++) |
Dannis_mbed | 0:81b21910454e | 419 | { |
Dannis_mbed | 0:81b21910454e | 420 | if (string [i] != 0x00) |
Dannis_mbed | 0:81b21910454e | 421 | { |
Dannis_mbed | 0:81b21910454e | 422 | vLCD_data_I2C (string [i]); |
Dannis_mbed | 0:81b21910454e | 423 | } |
Dannis_mbed | 0:81b21910454e | 424 | else |
Dannis_mbed | 0:81b21910454e | 425 | vLCD_data_I2C (' '); |
Dannis_mbed | 0:81b21910454e | 426 | } |
Dannis_mbed | 0:81b21910454e | 427 | } |
Dannis_mbed | 0:81b21910454e | 428 | |
Dannis_mbed | 0:81b21910454e | 429 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 430 | // Write string to LCD // |
Dannis_mbed | 0:81b21910454e | 431 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 432 | |
Dannis_mbed | 0:81b21910454e | 433 | void vLCD_printPos_I2C (unsigned char *string, unsigned char line, unsigned char character) |
Dannis_mbed | 0:81b21910454e | 434 | { |
Dannis_mbed | 0:81b21910454e | 435 | unsigned int i; |
Dannis_mbed | 0:81b21910454e | 436 | |
Dannis_mbed | 0:81b21910454e | 437 | // select target line and position to write to |
Dannis_mbed | 0:81b21910454e | 438 | if (line == 1) |
Dannis_mbed | 0:81b21910454e | 439 | { |
Dannis_mbed | 0:81b21910454e | 440 | vLCD_cmd_I2C (0x80+(character-1)); |
Dannis_mbed | 0:81b21910454e | 441 | } |
Dannis_mbed | 0:81b21910454e | 442 | |
Dannis_mbed | 0:81b21910454e | 443 | else if (line == 2) |
Dannis_mbed | 0:81b21910454e | 444 | { |
Dannis_mbed | 0:81b21910454e | 445 | vLCD_cmd_I2C (0xC0+(character-1)); |
Dannis_mbed | 0:81b21910454e | 446 | } |
Dannis_mbed | 0:81b21910454e | 447 | |
Dannis_mbed | 0:81b21910454e | 448 | else |
Dannis_mbed | 0:81b21910454e | 449 | { |
Dannis_mbed | 0:81b21910454e | 450 | return; // End of function: Error in the past value of line |
Dannis_mbed | 0:81b21910454e | 451 | } |
Dannis_mbed | 0:81b21910454e | 452 | |
Dannis_mbed | 0:81b21910454e | 453 | // write data to selected line and position |
Dannis_mbed | 0:81b21910454e | 454 | for (i=0; i<16; i++) |
Dannis_mbed | 0:81b21910454e | 455 | { |
Dannis_mbed | 0:81b21910454e | 456 | if (string [i] != 0x00) |
Dannis_mbed | 0:81b21910454e | 457 | { |
Dannis_mbed | 0:81b21910454e | 458 | vLCD_data_I2C (string [i]); |
Dannis_mbed | 0:81b21910454e | 459 | } |
Dannis_mbed | 0:81b21910454e | 460 | else |
Dannis_mbed | 0:81b21910454e | 461 | vLCD_data_I2C (' '); |
Dannis_mbed | 0:81b21910454e | 462 | } |
Dannis_mbed | 0:81b21910454e | 463 | } |
Dannis_mbed | 0:81b21910454e | 464 | |
Dannis_mbed | 0:81b21910454e | 465 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 466 | // update LCD // |
Dannis_mbed | 0:81b21910454e | 467 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 468 | void vLCD_clear_I2C (void) |
Dannis_mbed | 0:81b21910454e | 469 | { |
Dannis_mbed | 0:81b21910454e | 470 | // Clear Display |
Dannis_mbed | 0:81b21910454e | 471 | vLCD_cmd_I2C (0x01); |
Dannis_mbed | 0:81b21910454e | 472 | //vLCD_delay_I2C (65535) |
Dannis_mbed | 0:81b21910454e | 473 | vLCD_delay_I2C (100); |
Dannis_mbed | 0:81b21910454e | 474 | // Display ON / OFF |
Dannis_mbed | 0:81b21910454e | 475 | vLCD_cmd_I2C (0x0C); |
Dannis_mbed | 0:81b21910454e | 476 | //vLCD_delay_I2C (65535) |
Dannis_mbed | 0:81b21910454e | 477 | vLCD_delay_I2C (100); |
Dannis_mbed | 0:81b21910454e | 478 | } |
Dannis_mbed | 0:81b21910454e | 479 | |
Dannis_mbed | 0:81b21910454e | 480 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 481 | // update LCD // |
Dannis_mbed | 0:81b21910454e | 482 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 483 | void vLCD_update (void) |
Dannis_mbed | 0:81b21910454e | 484 | { |
Dannis_mbed | 0:81b21910454e | 485 | vLCD_print_I2C((unsigned char*)mLCDMenu.getLine1(), 1); |
Dannis_mbed | 0:81b21910454e | 486 | vLCD_print_I2C((unsigned char*)mLCDMenu.getLine2(), 2); |
Dannis_mbed | 0:81b21910454e | 487 | //vLCD_printPos_I2C((unsigned char*)mLCDMenu.getLine1(), 1, 1); |
Dannis_mbed | 0:81b21910454e | 488 | //vLCD_printPos_I2C((unsigned char*)mLCDMenu.getLine2(), 2, 1); |
Dannis_mbed | 0:81b21910454e | 489 | } |
Dannis_mbed | 0:81b21910454e | 490 | |
Dannis_mbed | 0:81b21910454e | 491 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 492 | // Init SOF module // |
Dannis_mbed | 0:81b21910454e | 493 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 494 | void vInit (void) |
Dannis_mbed | 0:81b21910454e | 495 | { |
Dannis_mbed | 0:81b21910454e | 496 | // I2C init |
Dannis_mbed | 0:81b21910454e | 497 | i2c.frequency(100000); |
Dannis_mbed | 0:81b21910454e | 498 | //Port A is databus - Output |
Dannis_mbed | 0:81b21910454e | 499 | mcp23017.direction(PORT_A, PORT_DIR_OUT); |
Dannis_mbed | 0:81b21910454e | 500 | //Port B is controlbus - Output |
Dannis_mbed | 0:81b21910454e | 501 | mcp23017.direction(PORT_B, PORT_DIR_OUT); |
Dannis_mbed | 0:81b21910454e | 502 | |
Dannis_mbed | 0:81b21910454e | 503 | debug.baud(115200); |
Dannis_mbed | 0:81b21910454e | 504 | // initialize LCD |
Dannis_mbed | 0:81b21910454e | 505 | vLCD_init_I2C(); |
Dannis_mbed | 1:b4a0d63db637 | 506 | mLCDMenu.vSelectMenu(_MENU0_0); |
Dannis_mbed | 0:81b21910454e | 507 | vLCD_update(); |
Dannis_mbed | 0:81b21910454e | 508 | |
Dannis_mbed | 0:81b21910454e | 509 | // led's out |
Dannis_mbed | 0:81b21910454e | 510 | aliveLed = 1; wait(0.1); aliveLed = 0; |
Dannis_mbed | 0:81b21910454e | 511 | statusLed = 1; wait(0.1); statusLed = 0; |
Dannis_mbed | 0:81b21910454e | 512 | emergencyLed = 1; wait(0.1); emergencyLed = 0; |
Dannis_mbed | 0:81b21910454e | 513 | |
Dannis_mbed | 0:81b21910454e | 514 | wait(0.2); |
Dannis_mbed | 0:81b21910454e | 515 | motorRelais = 1; |
Dannis_mbed | 0:81b21910454e | 516 | wait(1.0); |
Dannis_mbed | 0:81b21910454e | 517 | motorRelais = 0; |
Dannis_mbed | 0:81b21910454e | 518 | |
Dannis_mbed | 0:81b21910454e | 519 | //Int-Handler rotary encoder switch |
Dannis_mbed | 0:81b21910454e | 520 | // call trigger_sw() when button of rotary-encoder is pressed |
Dannis_mbed | 0:81b21910454e | 521 | rSwitch.attachSW(&trigger_sw); |
Dannis_mbed | 0:81b21910454e | 522 | // call trigger_rot() when the shaft is rotaded left or right |
Dannis_mbed | 0:81b21910454e | 523 | rSwitch.attachROT(&trigger_rotated); |
Dannis_mbed | 0:81b21910454e | 524 | lastGet = 0; |
Dannis_mbed | 0:81b21910454e | 525 | // set encrotated, so position is displayed on startup |
Dannis_mbed | 0:81b21910454e | 526 | enc_rotated = true; |
Dannis_mbed | 0:81b21910454e | 527 | // blink alive LED |
Dannis_mbed | 0:81b21910454e | 528 | aliveLed = 1; |
Dannis_mbed | 0:81b21910454e | 529 | tAliveOff.attach(&vBlinkOnce_cb, 0.05); |
Dannis_mbed | 0:81b21910454e | 530 | tAliveOn.attach(&vBlinkOnce_cb, 5); |
Dannis_mbed | 0:81b21910454e | 531 | } |
Dannis_mbed | 0:81b21910454e | 532 | |
Dannis_mbed | 0:81b21910454e | 533 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 534 | // Init MPU 9150 // |
Dannis_mbed | 0:81b21910454e | 535 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 536 | void vInitImu (void) |
Dannis_mbed | 0:81b21910454e | 537 | { |
Dannis_mbed | 1:b4a0d63db637 | 538 | if(imu.isReady()) mLCDMenu.vShowScreen(_IMUREADY); |
Dannis_mbed | 1:b4a0d63db637 | 539 | else mLCDMenu.vShowScreen(_IMUFAIL); |
Dannis_mbed | 0:81b21910454e | 540 | vLCD_update(); |
Dannis_mbed | 0:81b21910454e | 541 | |
Dannis_mbed | 0:81b21910454e | 542 | imu.initialiseDMP(); |
Dannis_mbed | 0:81b21910454e | 543 | imu.setFifoReset(true); |
Dannis_mbed | 0:81b21910454e | 544 | imu.setDMPEnabled(true); |
Dannis_mbed | 0:81b21910454e | 545 | intAGM.rise(&vGetMPUBuffer); |
Dannis_mbed | 0:81b21910454e | 546 | //tMPU.attach_us(&vGetMPUBuffer,50000); |
Dannis_mbed | 0:81b21910454e | 547 | } |
Dannis_mbed | 0:81b21910454e | 548 | |
Dannis_mbed | 0:81b21910454e | 549 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 550 | // Init CANbus // |
Dannis_mbed | 0:81b21910454e | 551 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 552 | void vInitCANBus (void) |
Dannis_mbed | 0:81b21910454e | 553 | { |
Dannis_mbed | 0:81b21910454e | 554 | CANbus.frequency(250000); |
Dannis_mbed | 0:81b21910454e | 555 | debug.printf("vInitCANBus\r\n"); |
Dannis_mbed | 0:81b21910454e | 556 | CANMessage msg; |
Dannis_mbed | 0:81b21910454e | 557 | int i, iTry = 10; |
Dannis_mbed | 0:81b21910454e | 558 | uint32_t id = MESSAGE_ALL; |
Dannis_mbed | 0:81b21910454e | 559 | bool bReceived = false; |
Dannis_mbed | 0:81b21910454e | 560 | cMessage = 0x00; |
Dannis_mbed | 0:81b21910454e | 561 | // check CAN system |
Dannis_mbed | 0:81b21910454e | 562 | for (i = 6; i > 0 ; i--) |
Dannis_mbed | 0:81b21910454e | 563 | { |
Dannis_mbed | 0:81b21910454e | 564 | debug.printf("for loop: %i\r\n", i); |
Dannis_mbed | 0:81b21910454e | 565 | bReceived = false; |
Dannis_mbed | 0:81b21910454e | 566 | id++; // increase id |
Dannis_mbed | 0:81b21910454e | 567 | while ((iTry > 0)&&(!bReceived)) |
Dannis_mbed | 0:81b21910454e | 568 | { |
Dannis_mbed | 0:81b21910454e | 569 | CANbus.write(CANMessage(id, &cMessage, 1)); |
Dannis_mbed | 0:81b21910454e | 570 | wait(0.15); |
Dannis_mbed | 0:81b21910454e | 571 | switch(id){ // set adress to filter |
Dannis_mbed | 0:81b21910454e | 572 | case PORT_ACT_DIAGN: uiFilterId = ACTUATOR_PORT_ID; break; |
Dannis_mbed | 0:81b21910454e | 573 | case STARB_ACT_DIAGN: uiFilterId = ACTUATOR_STARB_ID; break; |
Dannis_mbed | 0:81b21910454e | 574 | case PORT_HGHT_DIAGN: uiFilterId = HEIGHT_PORT_ID; break; |
Dannis_mbed | 0:81b21910454e | 575 | case STARB_HGHT_DIAGN: uiFilterId = HEIGHT_STARB_ID; break; |
Dannis_mbed | 0:81b21910454e | 576 | case GYRO_GPS_DIAGN: uiFilterId = GYRO_GPS_ID; break; |
Dannis_mbed | 0:81b21910454e | 577 | case EXT_UI_DIAGN: uiFilterId = EXT_UI_ID; break; |
Dannis_mbed | 0:81b21910454e | 578 | }//end switch |
Dannis_mbed | 0:81b21910454e | 579 | iFilter = CANbus.filter(0x200, 0x7FF, CANStandard); |
Dannis_mbed | 0:81b21910454e | 580 | if(CANbus.read(msg, iFilter)){ |
Dannis_mbed | 0:81b21910454e | 581 | debug.printf("can.read(msg) ID: %i\r\n", id); |
Dannis_mbed | 0:81b21910454e | 582 | debug.printf("can.msg id: %i, %i\r\n", msg.id, msg.data[0]); |
Dannis_mbed | 0:81b21910454e | 583 | switch(id){ |
Dannis_mbed | 1:b4a0d63db637 | 584 | case PORT_ACT_DIAGN: if((msg.id == ACTUATOR_PORT_ID)&&(msg.data[0] == 0xFF)) mLCDMenu.vShowScreen(_CANID101OK); break; |
Dannis_mbed | 1:b4a0d63db637 | 585 | case STARB_ACT_DIAGN: if((msg.id == ACTUATOR_STARB_ID)&&(msg.data[0] == 0xFF)) mLCDMenu.vShowScreen(_CANID102OK); break; |
Dannis_mbed | 1:b4a0d63db637 | 586 | case PORT_HGHT_DIAGN: if((msg.id == HEIGHT_PORT_ID)&&(msg.data[0] == 0xFF)) mLCDMenu.vShowScreen(_CANID201OK); break; |
Dannis_mbed | 1:b4a0d63db637 | 587 | case STARB_HGHT_DIAGN: if((msg.id == HEIGHT_STARB_ID)&&(msg.data[0] == 0xFF)) mLCDMenu.vShowScreen(_CANID202OK); break; |
Dannis_mbed | 1:b4a0d63db637 | 588 | case GYRO_GPS_DIAGN: if((msg.id == GYRO_GPS_ID)&&(msg.data[0] == 0xFF)) mLCDMenu.vShowScreen(_CANID205OK); break; |
Dannis_mbed | 1:b4a0d63db637 | 589 | case EXT_UI_DIAGN: if((msg.id == EXT_UI_ID)&&(msg.data[0] == 0xFF)) mLCDMenu.vShowScreen(_CANID1001OK); break; |
Dannis_mbed | 0:81b21910454e | 590 | } //end switch |
Dannis_mbed | 0:81b21910454e | 591 | vLCD_update(); |
Dannis_mbed | 0:81b21910454e | 592 | bReceived = true; |
Dannis_mbed | 0:81b21910454e | 593 | } //end if |
Dannis_mbed | 0:81b21910454e | 594 | else if(iTry == 1){ |
Dannis_mbed | 0:81b21910454e | 595 | switch(id){ |
Dannis_mbed | 1:b4a0d63db637 | 596 | case PORT_ACT_DIAGN: mLCDMenu.vShowScreen(_CANID101FAIL); break; |
Dannis_mbed | 1:b4a0d63db637 | 597 | case STARB_ACT_DIAGN: mLCDMenu.vShowScreen(_CANID102FAIL); break; |
Dannis_mbed | 1:b4a0d63db637 | 598 | case PORT_HGHT_DIAGN: mLCDMenu.vShowScreen(_CANID201FAIL); break; |
Dannis_mbed | 1:b4a0d63db637 | 599 | case STARB_HGHT_DIAGN: mLCDMenu.vShowScreen(_CANID202FAIL); break; |
Dannis_mbed | 1:b4a0d63db637 | 600 | case GYRO_GPS_DIAGN: mLCDMenu.vShowScreen(_CANID205FAIL); break; |
Dannis_mbed | 1:b4a0d63db637 | 601 | case EXT_UI_DIAGN: mLCDMenu.vShowScreen(_CANID1001FAIL); break; |
Dannis_mbed | 0:81b21910454e | 602 | } //end switch |
Dannis_mbed | 0:81b21910454e | 603 | vLCD_update(); |
Dannis_mbed | 0:81b21910454e | 604 | } //end if |
Dannis_mbed | 0:81b21910454e | 605 | if(CANbus.read(msg)){}//unwanted message |
Dannis_mbed | 0:81b21910454e | 606 | if(!bReceived) iTry--; |
Dannis_mbed | 0:81b21910454e | 607 | debug.printf("Tries: %i\r\n", iTry); |
Dannis_mbed | 0:81b21910454e | 608 | } //end while |
Dannis_mbed | 0:81b21910454e | 609 | wait(0.5); // some time to read message |
Dannis_mbed | 0:81b21910454e | 610 | iTry = 10; // reset try counter |
Dannis_mbed | 0:81b21910454e | 611 | // *****afhandelen can fail **** |
Dannis_mbed | 0:81b21910454e | 612 | if (iTry != 0) iTry = 10; |
Dannis_mbed | 0:81b21910454e | 613 | else bSystemFail = true; |
Dannis_mbed | 0:81b21910454e | 614 | } //end for |
Dannis_mbed | 0:81b21910454e | 615 | |
Dannis_mbed | 0:81b21910454e | 616 | // in case system fail |
Dannis_mbed | 0:81b21910454e | 617 | if(bSystemFail){ |
Dannis_mbed | 0:81b21910454e | 618 | vCallEmergencyLed(); |
Dannis_mbed | 1:b4a0d63db637 | 619 | mLCDMenu.vShowScreen(_CANNORESPONSE); |
Dannis_mbed | 1:b4a0d63db637 | 620 | } |
Dannis_mbed | 1:b4a0d63db637 | 621 | cMessage = (char) svSoF.iGetHeightFoilBorne(); |
Dannis_mbed | 1:b4a0d63db637 | 622 | CANbus.write(CANMessage(EXT_UI_HEIGHT, &cMessage, 1)); |
Dannis_mbed | 1:b4a0d63db637 | 623 | |
Dannis_mbed | 1:b4a0d63db637 | 624 | mLCDMenu.vSelectMenu(_MENU0_0); |
Dannis_mbed | 1:b4a0d63db637 | 625 | vLCD_update(); |
Dannis_mbed | 0:81b21910454e | 626 | debug.printf("end can init"); |
Dannis_mbed | 0:81b21910454e | 627 | } |
Dannis_mbed | 0:81b21910454e | 628 | |
Dannis_mbed | 0:81b21910454e | 629 | // call vCANBusSend every 1 second test |
Dannis_mbed | 0:81b21910454e | 630 | // CANTicker.attach(&vCANBusSend, 1); |
Dannis_mbed | 0:81b21910454e | 631 | |
Dannis_mbed | 0:81b21910454e | 632 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 633 | // Blink alive led // |
Dannis_mbed | 0:81b21910454e | 634 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 635 | void vBlinkOff_cb (void) |
Dannis_mbed | 0:81b21910454e | 636 | { |
Dannis_mbed | 0:81b21910454e | 637 | aliveLed = 0; |
Dannis_mbed | 0:81b21910454e | 638 | } |
Dannis_mbed | 0:81b21910454e | 639 | |
Dannis_mbed | 0:81b21910454e | 640 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 641 | // Blink emergecy led // |
Dannis_mbed | 0:81b21910454e | 642 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 643 | void vEmergencyOff_cb (void) |
Dannis_mbed | 0:81b21910454e | 644 | { |
Dannis_mbed | 0:81b21910454e | 645 | emergencyLed = 0; |
Dannis_mbed | 0:81b21910454e | 646 | } |
Dannis_mbed | 0:81b21910454e | 647 | |
Dannis_mbed | 0:81b21910454e | 648 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 649 | // Blink status led once // |
Dannis_mbed | 0:81b21910454e | 650 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 651 | void vBlinkOnce_cb (void) |
Dannis_mbed | 0:81b21910454e | 652 | { |
Dannis_mbed | 0:81b21910454e | 653 | aliveLed = 1; |
Dannis_mbed | 0:81b21910454e | 654 | tAliveOff.detach(); |
Dannis_mbed | 0:81b21910454e | 655 | tAliveOff.attach(&vBlinkOff_cb, 0.05); |
Dannis_mbed | 0:81b21910454e | 656 | } |
Dannis_mbed | 0:81b21910454e | 657 | |
Dannis_mbed | 0:81b21910454e | 658 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 659 | // Blink emergency led once // |
Dannis_mbed | 0:81b21910454e | 660 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 661 | void vEmergencyOnce_cb (void) |
Dannis_mbed | 0:81b21910454e | 662 | { |
Dannis_mbed | 0:81b21910454e | 663 | emergencyLed = 1; |
Dannis_mbed | 0:81b21910454e | 664 | tEmergencyLedOff.detach(); |
Dannis_mbed | 0:81b21910454e | 665 | tEmergencyLedOff.attach(&vEmergencyOff_cb, 0.05); |
Dannis_mbed | 0:81b21910454e | 666 | |
Dannis_mbed | 0:81b21910454e | 667 | if (!bSystemFail) |
Dannis_mbed | 0:81b21910454e | 668 | { |
Dannis_mbed | 0:81b21910454e | 669 | tEmergencyLedOn.detach(); |
Dannis_mbed | 0:81b21910454e | 670 | tEmergencyLedOff.detach(); |
Dannis_mbed | 0:81b21910454e | 671 | aliveLed = 0; |
Dannis_mbed | 0:81b21910454e | 672 | } |
Dannis_mbed | 0:81b21910454e | 673 | } |
Dannis_mbed | 0:81b21910454e | 674 | |
Dannis_mbed | 0:81b21910454e | 675 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 676 | // Call emergency led // |
Dannis_mbed | 0:81b21910454e | 677 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 678 | void vCallEmergencyLed(void) |
Dannis_mbed | 0:81b21910454e | 679 | { |
Dannis_mbed | 0:81b21910454e | 680 | aliveLed = 1; |
Dannis_mbed | 0:81b21910454e | 681 | tEmergencyLedOff.attach(&vEmergencyOnce_cb, 0.05); |
Dannis_mbed | 0:81b21910454e | 682 | tEmergencyLedOn.attach(&vEmergencyOnce_cb, 0.5); |
Dannis_mbed | 0:81b21910454e | 683 | } |
Dannis_mbed | 0:81b21910454e | 684 | |
Dannis_mbed | 0:81b21910454e | 685 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 686 | // Writing integer to LCD // |
Dannis_mbed | 0:81b21910454e | 687 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 688 | void vLCD_printInt_I2C (int value, unsigned char line, unsigned char character) |
Dannis_mbed | 0:81b21910454e | 689 | { |
Dannis_mbed | 0:81b21910454e | 690 | static char buffer[32]; |
Dannis_mbed | 0:81b21910454e | 691 | // convert int to char |
Dannis_mbed | 0:81b21910454e | 692 | sprintf(buffer, "%.2d", value); |
Dannis_mbed | 0:81b21910454e | 693 | // write converted int to LCD |
Dannis_mbed | 0:81b21910454e | 694 | vLCD_printPos_I2C((unsigned char*)buffer, line, character); |
Dannis_mbed | 0:81b21910454e | 695 | } |
Dannis_mbed | 0:81b21910454e | 696 | |
Dannis_mbed | 0:81b21910454e | 697 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 698 | // calculate Roll from quaternion // |
Dannis_mbed | 0:81b21910454e | 699 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 700 | float getRollAngle(Quaternion q1) |
Dannis_mbed | 0:81b21910454e | 701 | { |
Dannis_mbed | 0:81b21910454e | 702 | float fRoll; |
Dannis_mbed | 0:81b21910454e | 703 | fRoll = atan2(2*(q1.v.x*q1.v.y + q1.w*q1.v.y), q1.w*q1.w + q1.v.x*q1.v.x - q1.v.y*q1.v.y - q1.v.y*q1.v.y); |
Dannis_mbed | 0:81b21910454e | 704 | fRoll *= 180/3.14; |
Dannis_mbed | 0:81b21910454e | 705 | return (fRoll); |
Dannis_mbed | 0:81b21910454e | 706 | } |
Dannis_mbed | 0:81b21910454e | 707 | |
Dannis_mbed | 0:81b21910454e | 708 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 709 | // calculate Pitch from quaternion // |
Dannis_mbed | 0:81b21910454e | 710 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 711 | float getPitchAngle(Quaternion q1) |
Dannis_mbed | 0:81b21910454e | 712 | { |
Dannis_mbed | 0:81b21910454e | 713 | Vector3 euler = q1.getEulerAngles(); |
Dannis_mbed | 0:81b21910454e | 714 | //debug.printf("$ Pitch = %f #\t", euler.y); |
Dannis_mbed | 0:81b21910454e | 715 | float fPitch; |
Dannis_mbed | 0:81b21910454e | 716 | //fPitch = atan2((2*((q1.v.x*q1.v.z) - (q1.w*q1.v.y))),(sqrt((2*((q1.w*q1.v.x) + (q1.v.y*q1.v.z)))*(2*((q1.w*q1.v.x) + (q1.v.y*q1.v.z)))) + (((q1.w*q1.w) - (q1.v.x*q1.v.x) - (q1.v.y*q1.v.y) + (q1.v.z*q1.v.z))*((q1.w*q1.w) - (q1.v.x*q1.v.x) - (q1.v.y*q1.v.y) + (q1.v.z*q1.v.z))))); |
Dannis_mbed | 0:81b21910454e | 717 | //fPitch = acos(-(q1.w*q1.w) - (q1.v.x*q1.v.x) + (q1.v.y*q1.v.y) + (q1.v.z*q1.v.z)); |
Dannis_mbed | 0:81b21910454e | 718 | //fPitch = -asin(2*q1.w*q1.v.y - 2*q1.v.x*q1.v.z); |
Dannis_mbed | 0:81b21910454e | 719 | fPitch = euler.y; |
Dannis_mbed | 0:81b21910454e | 720 | fPitch *= 180/3.14; |
Dannis_mbed | 0:81b21910454e | 721 | return (fPitch); |
Dannis_mbed | 0:81b21910454e | 722 | |
Dannis_mbed | 0:81b21910454e | 723 | } |
Dannis_mbed | 0:81b21910454e | 724 | |
Dannis_mbed | 0:81b21910454e | 725 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 726 | // interrup-Handler for button on rotary-encoder // |
Dannis_mbed | 0:81b21910454e | 727 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 728 | void trigger_sw(void) |
Dannis_mbed | 0:81b21910454e | 729 | { |
Dannis_mbed | 0:81b21910454e | 730 | enc_pressed = true; // just set the flag, rest is done outside isr |
Dannis_mbed | 0:81b21910454e | 731 | } |
Dannis_mbed | 0:81b21910454e | 732 | |
Dannis_mbed | 0:81b21910454e | 733 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 734 | // interrup-Handler for rotary-encoder rotation // |
Dannis_mbed | 0:81b21910454e | 735 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 736 | void trigger_rotated(void) |
Dannis_mbed | 0:81b21910454e | 737 | { |
Dannis_mbed | 0:81b21910454e | 738 | enc_rotated = true; // just set the flag, rest is done outside isr |
Dannis_mbed | 0:81b21910454e | 739 | } |
Dannis_mbed | 0:81b21910454e | 740 | |
Dannis_mbed | 0:81b21910454e | 741 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 742 | // reset for rotary-encoder switch // |
Dannis_mbed | 0:81b21910454e | 743 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 744 | void vResetSwitch (void) |
Dannis_mbed | 0:81b21910454e | 745 | { |
Dannis_mbed | 0:81b21910454e | 746 | rSwitch.Set(0); |
Dannis_mbed | 0:81b21910454e | 747 | } |
Dannis_mbed | 0:81b21910454e | 748 | |
Dannis_mbed | 0:81b21910454e | 749 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 750 | // set for rotary-encoder switch // |
Dannis_mbed | 0:81b21910454e | 751 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 752 | void vSetSwitch (int value) |
Dannis_mbed | 0:81b21910454e | 753 | { |
Dannis_mbed | 0:81b21910454e | 754 | rSwitch.Set(value); |
Dannis_mbed | 0:81b21910454e | 755 | } |
Dannis_mbed | 0:81b21910454e | 756 | |
Dannis_mbed | 0:81b21910454e | 757 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 758 | // rotary-encoder switch rotated // |
Dannis_mbed | 0:81b21910454e | 759 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 760 | void vSwitchRotated (void) |
Dannis_mbed | 0:81b21910454e | 761 | { |
Dannis_mbed | 0:81b21910454e | 762 | enc_rotated = false; |
Dannis_mbed | 1:b4a0d63db637 | 763 | |
Dannis_mbed | 0:81b21910454e | 764 | // get value from rotary encoder |
Dannis_mbed | 0:81b21910454e | 765 | thisGet = rSwitch.Get(); |
Dannis_mbed | 1:b4a0d63db637 | 766 | |
Dannis_mbed | 1:b4a0d63db637 | 767 | if(rSwitch.Get()<0) mLCDMenu.vRotaryDown(); |
Dannis_mbed | 1:b4a0d63db637 | 768 | else if(rSwitch.Get()>=0) mLCDMenu.vRotaryUp(); |
Dannis_mbed | 1:b4a0d63db637 | 769 | |
Dannis_mbed | 1:b4a0d63db637 | 770 | // reset switch count |
Dannis_mbed | 1:b4a0d63db637 | 771 | vResetSwitch(); |
Dannis_mbed | 0:81b21910454e | 772 | vLCD_update(); |
Dannis_mbed | 0:81b21910454e | 773 | } |
Dannis_mbed | 0:81b21910454e | 774 | |
Dannis_mbed | 0:81b21910454e | 775 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 776 | // rotary-encoder switch confirmed // |
Dannis_mbed | 0:81b21910454e | 777 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 778 | void vSwitchConfirmed (void) |
Dannis_mbed | 0:81b21910454e | 779 | { |
Dannis_mbed | 0:81b21910454e | 780 | enc_pressed = false; |
Dannis_mbed | 0:81b21910454e | 781 | |
Dannis_mbed | 1:b4a0d63db637 | 782 | mLCDMenu.vRotaryConfirm(); |
Dannis_mbed | 1:b4a0d63db637 | 783 | |
Dannis_mbed | 0:81b21910454e | 784 | // reset switch count |
Dannis_mbed | 0:81b21910454e | 785 | vResetSwitch(); |
Dannis_mbed | 1:b4a0d63db637 | 786 | |
Dannis_mbed | 1:b4a0d63db637 | 787 | vLCD_update(); |
Dannis_mbed | 0:81b21910454e | 788 | } |
Dannis_mbed | 0:81b21910454e | 789 | |
Dannis_mbed | 0:81b21910454e | 790 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 791 | // CANbus send not used // |
Dannis_mbed | 0:81b21910454e | 792 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 793 | void vCANBusSend(void) |
Dannis_mbed | 0:81b21910454e | 794 | { |
Dannis_mbed | 0:81b21910454e | 795 | static char counter = 0; |
Dannis_mbed | 0:81b21910454e | 796 | static char counterTwo = 0; |
Dannis_mbed | 0:81b21910454e | 797 | static char random[8] = {0}; |
Dannis_mbed | 0:81b21910454e | 798 | //vLCD_printPos_I2C((unsigned char*)"CANbus send ", 2, 1); |
Dannis_mbed | 0:81b21910454e | 799 | switch(messageToSend){ |
Dannis_mbed | 0:81b21910454e | 800 | case MSG1: |
Dannis_mbed | 0:81b21910454e | 801 | if(CANbus.write(CANMessage(1336, &counter, 1))) |
Dannis_mbed | 0:81b21910454e | 802 | { |
Dannis_mbed | 0:81b21910454e | 803 | counter++; |
Dannis_mbed | 0:81b21910454e | 804 | messageToSend = MSG2; |
Dannis_mbed | 0:81b21910454e | 805 | break; |
Dannis_mbed | 0:81b21910454e | 806 | } |
Dannis_mbed | 0:81b21910454e | 807 | case MSG2: |
Dannis_mbed | 0:81b21910454e | 808 | if(CANbus.write(CANMessage(1003, &counterTwo, 1))) |
Dannis_mbed | 0:81b21910454e | 809 | { |
Dannis_mbed | 0:81b21910454e | 810 | counterTwo++; |
Dannis_mbed | 0:81b21910454e | 811 | messageToSend = MSG3; |
Dannis_mbed | 0:81b21910454e | 812 | break; |
Dannis_mbed | 0:81b21910454e | 813 | } |
Dannis_mbed | 0:81b21910454e | 814 | case MSG3: |
Dannis_mbed | 0:81b21910454e | 815 | if(CANbus.write(CANMessage(1236, random, 4))) |
Dannis_mbed | 0:81b21910454e | 816 | { |
Dannis_mbed | 0:81b21910454e | 817 | random[0] = rand()% 10 + 1; |
Dannis_mbed | 0:81b21910454e | 818 | random[1] = rand()% 10 + 20; |
Dannis_mbed | 0:81b21910454e | 819 | random[2] = rand()% 100 + 1; |
Dannis_mbed | 0:81b21910454e | 820 | random[3] = 00; |
Dannis_mbed | 0:81b21910454e | 821 | //messageToSend = MSG1; |
Dannis_mbed | 0:81b21910454e | 822 | break; |
Dannis_mbed | 0:81b21910454e | 823 | } |
Dannis_mbed | 0:81b21910454e | 824 | } // end switch case |
Dannis_mbed | 0:81b21910454e | 825 | |
Dannis_mbed | 0:81b21910454e | 826 | myLed = !myLed; |
Dannis_mbed | 0:81b21910454e | 827 | } |
Dannis_mbed | 0:81b21910454e | 828 | |
Dannis_mbed | 0:81b21910454e | 829 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 830 | // Get data from MPU fifo buffer // |
Dannis_mbed | 0:81b21910454e | 831 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 832 | void vGetMPUBuffer(void) |
Dannis_mbed | 0:81b21910454e | 833 | { |
Dannis_mbed | 0:81b21910454e | 834 | int iFifoCount; |
Dannis_mbed | 0:81b21910454e | 835 | if (imu.getInterruptFifoOverflow()) |
Dannis_mbed | 0:81b21910454e | 836 | imu.setFifoReset(true); |
Dannis_mbed | 0:81b21910454e | 837 | iFifoCount = imu.getFifoCount(); |
Dannis_mbed | 0:81b21910454e | 838 | //debug.printf("fifocount: %i\r\n", iFifoCount); |
Dannis_mbed | 0:81b21910454e | 839 | if(imu.getFifoCount() >= 48){ |
Dannis_mbed | 0:81b21910454e | 840 | if(iFifoCount % 48 != 0) imu.getFifoBuffer(cBuffer, iFifoCount % 48); |
Dannis_mbed | 0:81b21910454e | 841 | //imu.getFifoBuffer(cBuffer, iFifoCount- 48); |
Dannis_mbed | 0:81b21910454e | 842 | imu.getFifoBuffer(cBuffer, 48); |
Dannis_mbed | 0:81b21910454e | 843 | //myLed2 = !myLed2; |
Dannis_mbed | 0:81b21910454e | 844 | q1.decode(cBuffer);// quaternion vector to buffer |
Dannis_mbed | 0:81b21910454e | 845 | } |
Dannis_mbed | 0:81b21910454e | 846 | } |
Dannis_mbed | 0:81b21910454e | 847 | |
Dannis_mbed | 0:81b21910454e | 848 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 849 | // EOF // |
Dannis_mbed | 0:81b21910454e | 850 | ////////////////////////////////////////////////////////////////////////////////////// |
Dannis_mbed | 0:81b21910454e | 851 | |
Dannis_mbed | 0:81b21910454e | 852 | /*if (fifoCount == 1024) // test |
Dannis_mbed | 0:81b21910454e | 853 | {mpu.resetFIFO();} // reset so we can continue cleanly |
Dannis_mbed | 0:81b21910454e | 854 | |
Dannis_mbed | 0:81b21910454e | 855 | else if (fifoCount >= 42) // otherwise, check for DMP data ready interrupt (this should happen frequently) |
Dannis_mbed | 0:81b21910454e | 856 | { |
Dannis_mbed | 0:81b21910454e | 857 | if ((fifoCount % stMPU9150->packetSize) != 0) // test |
Dannis_mbed | 0:81b21910454e | 858 | {mpu.getFIFOBytes(fifoBuffer, (fifoCount % stMPU9150->packetSize));} // test |
Dannis_mbed | 0:81b21910454e | 859 | |
Dannis_mbed | 0:81b21910454e | 860 | while (mpu.getFIFOCount() >= 42) |
Dannis_mbed | 0:81b21910454e | 861 | {mpu.getFIFOBytes(fifoBuffer, stMPU9150->packetSize);} // read a packet from FIFO |
Dannis_mbed | 0:81b21910454e | 862 | |
Dannis_mbed | 0:81b21910454e | 863 | mpu.dmpGetQuaternion(&stMPU9150->q, fifoBuffer);*/ |