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@0:81b21910454e, 2015-06-23 (annotated)

Committer:
Dannis_mbed
Date:
Tue Jun 23 13:55:28 2015 +0000
Revision:
0:81b21910454e
Child:
1:b4a0d63db637
version 1.0

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