HAN Solarboat / Mbed 2 deprecated SolarOnFoils_MainModule_20150518

version 1.0

Dependencies:   CMSIS_DSP_401 GPS MPU9150_DMP PID QuaternionMath Servo mbed

Fork of SolarOnFoils_MainModule_20150518 by Dannis Brugman

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?

UserRevisionLine numberNew 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);*/