HAN Solarboat
version 1.0
Dependencies: CMSIS_DSP_401 GPS MPU9150_DMP PID QuaternionMath Servo mbed
Fork of
SolarOnFoils_MainModule_20150518
by 
Dannis Brugman
Diff: main.cpp
- Revision:
- 1:b4a0d63db637
- Parent:
- 0:81b21910454e
- Child:
- 2:f6d058931b17
--- a/main.cpp Tue Jun 23 13:55:28 2015 +0000
+++ b/main.cpp Fri Jun 26 09:21:33 2015 +0000
@@ -31,7 +31,7 @@
#include "Servo.h"
#include "mRotaryEncoder.h"
#include "MainModule.h"
-#include "LCD_Menu.h"
+//#include "LCD_Menu.h"
#include "menu.h"
#include "systemVar.h"
@@ -43,12 +43,17 @@
#define PROJECT "\033[1;10fSolar on Foils\n"
#define VERSION "\033[2;10fVersion 1.0\n"
+#define EXT_UI_ID 20
#define ACTUATOR_PORT_ID 101
#define ACTUATOR_STARB_ID 102
+#define ACTUATOR_PORT_RUN 103
+#define ACTUATOR_STARB_RUN 104
#define HEIGHT_PORT_ID 201
#define HEIGHT_STARB_ID 202
+#define HEIGHT_PORT_DATA 203
+#define HEIGHT_STARB_DATA 204
#define GYRO_GPS_ID 205
-#define EXT_UI_ID 20
+#define EXT_UI_HEIGHT 1020
#define MESSAGE_ALL 2000
#define PORT_ACT_DIAGN 2001
#define STARB_ACT_DIAGN 2002
@@ -123,6 +128,7 @@
int thisGet;
int iFilter;
uint32_t uiFilterId;
+
bool enc_pressed = false; // Button of rotaryencoder was pressed
bool enc_rotated = false; // rotary encoder was totaded left or right
@@ -132,6 +138,8 @@
char cBuffer[200];
char cMessage;
char text[16];
+char cPTurns[2] ={0};
+char cSTurns[2] ={0};
//char buffer[16];
float fRoll, fPitch, fEulerRoll;
@@ -192,7 +200,7 @@
// put text on lcd
- //mLCDMenu.cShowScreen(_FOILBORNE);
+ //mLCDMenu.vShowScreen(_FOILBORNE);
//vLCD_printPos_I2C((unsigned char*)mLCDMenu.getLine1(), 2, 1);
//////////////////////////////////////////////////////////////////////////////////////
@@ -206,17 +214,47 @@
// rotary switch confirm pressed?
if (enc_pressed) vSwitchConfirmed();
- // answer on id#1003
+ // answer CAN
+ if(CANbus.read(msg)){
+ if (msg.id == EXT_UI_HEIGHT) svSoF.vVarHeightFoilBorne((uint32_t) msg.data[0]);
+ }
if(CANbus.read(msg)){
- if ((msg.id == ACTUATOR_PORT_ID)&&(msg.data[0] == 0xFF)) statusLed = ! statusLed;
+ debug.printf("ID: %i.\t", msg.id);
+ if ((msg.id == HEIGHT_PORT_DATA)&&(mLCDMenu.bGetReadHeight() == true))
+ {
+ svSoF.vSetPHeight((uint32_t) msg.data[0]);
+ debug.printf("height received: %c cm\r\n", msg.data[0]);
+ }
+ //if ((msg.id == ACTUATOR_PORT_ID)&&(msg.data[0] == 0xFF)) statusLed = ! statusLed;
//debug.printf("ID#102 received: %d answer.\r\n", msg.data[0]);
}
- if(timer.read_ms() > 500)
+ if(timer.read_ms() > 250)
{
timer.reset();
+ if(svSoF.uiGetRoll() >= 4)
+ {
+ if(svSoF.iGetRollPolarity() == 0)
+ {
+ cPTurns[1] = 1;
+ cSTurns[1] = 0;
+ }
+ else
+ {
+ cPTurns[1] = 0;
+ cSTurns[1] = 1;
+ }
+ cPTurns[0] = 1;
+ cSTurns[0] = 1;
+
+ CANbus.write(CANMessage(ACTUATOR_PORT_RUN, cPTurns, 2)); // send message to port actuator
+ wait(0.15);
+ CANbus.write(CANMessage(ACTUATOR_STARB_RUN, cSTurns, 2)); // send message to port actuator
+
+ }//end if
svSoF.vSetPitch(q1); // call function class SystemVar
- mLCDMenu.cShowScreen(_PITCHTEST);
+ svSoF.vSetRoll(q1);
+ mLCDMenu.vShowScreen(mLCDMenu.getScreen());
vLCD_update();
//debug.printf("Roll: %c\t", svSoF.getValue());
@@ -465,7 +503,7 @@
debug.baud(115200);
// initialize LCD
vLCD_init_I2C();
- mLCDMenu.cShowScreen(_INIT);
+ mLCDMenu.vSelectMenu(_MENU0_0);
vLCD_update();
// led's out
@@ -497,8 +535,8 @@
//////////////////////////////////////////////////////////////////////////////////////
void vInitImu (void)
{
- if(imu.isReady()) mLCDMenu.cShowScreen(_IMUREADY);
- else mLCDMenu.cShowScreen(_IMUFAIL);
+ if(imu.isReady()) mLCDMenu.vShowScreen(_IMUREADY);
+ else mLCDMenu.vShowScreen(_IMUFAIL);
vLCD_update();
imu.initialiseDMP();
@@ -543,24 +581,24 @@
debug.printf("can.read(msg) ID: %i\r\n", id);
debug.printf("can.msg id: %i, %i\r\n", msg.id, msg.data[0]);
switch(id){
- case PORT_ACT_DIAGN: if((msg.id == ACTUATOR_PORT_ID)&&(msg.data[0] == 0xFF)) mLCDMenu.cShowScreen(_CANID101OK); break;
- case STARB_ACT_DIAGN: if((msg.id == ACTUATOR_STARB_ID)&&(msg.data[0] == 0xFF)) mLCDMenu.cShowScreen(_CANID102OK); break;
- case PORT_HGHT_DIAGN: if((msg.id == HEIGHT_PORT_ID)&&(msg.data[0] == 0xFF)) mLCDMenu.cShowScreen(_CANID201OK); break;
- case STARB_HGHT_DIAGN: if((msg.id == HEIGHT_STARB_ID)&&(msg.data[0] == 0xFF)) mLCDMenu.cShowScreen(_CANID202OK); break;
- case GYRO_GPS_DIAGN: if((msg.id == GYRO_GPS_ID)&&(msg.data[0] == 0xFF)) mLCDMenu.cShowScreen(_CANID205OK); break;
- case EXT_UI_DIAGN: if((msg.id == EXT_UI_ID)&&(msg.data[0] == 0xFF)) mLCDMenu.cShowScreen(_CANID1001OK); break;
+ case PORT_ACT_DIAGN: if((msg.id == ACTUATOR_PORT_ID)&&(msg.data[0] == 0xFF)) mLCDMenu.vShowScreen(_CANID101OK); break;
+ case STARB_ACT_DIAGN: if((msg.id == ACTUATOR_STARB_ID)&&(msg.data[0] == 0xFF)) mLCDMenu.vShowScreen(_CANID102OK); break;
+ case PORT_HGHT_DIAGN: if((msg.id == HEIGHT_PORT_ID)&&(msg.data[0] == 0xFF)) mLCDMenu.vShowScreen(_CANID201OK); break;
+ case STARB_HGHT_DIAGN: if((msg.id == HEIGHT_STARB_ID)&&(msg.data[0] == 0xFF)) mLCDMenu.vShowScreen(_CANID202OK); break;
+ case GYRO_GPS_DIAGN: if((msg.id == GYRO_GPS_ID)&&(msg.data[0] == 0xFF)) mLCDMenu.vShowScreen(_CANID205OK); break;
+ case EXT_UI_DIAGN: if((msg.id == EXT_UI_ID)&&(msg.data[0] == 0xFF)) mLCDMenu.vShowScreen(_CANID1001OK); break;
} //end switch
vLCD_update();
bReceived = true;
} //end if
else if(iTry == 1){
switch(id){
- case PORT_ACT_DIAGN: mLCDMenu.cShowScreen(_CANID101FAIL); break;
- case STARB_ACT_DIAGN: mLCDMenu.cShowScreen(_CANID102FAIL); break;
- case PORT_HGHT_DIAGN: mLCDMenu.cShowScreen(_CANID201FAIL); break;
- case STARB_HGHT_DIAGN: mLCDMenu.cShowScreen(_CANID202FAIL); break;
- case GYRO_GPS_DIAGN: mLCDMenu.cShowScreen(_CANID205FAIL); break;
- case EXT_UI_DIAGN: mLCDMenu.cShowScreen(_CANID1001FAIL); break;
+ case PORT_ACT_DIAGN: mLCDMenu.vShowScreen(_CANID101FAIL); break;
+ case STARB_ACT_DIAGN: mLCDMenu.vShowScreen(_CANID102FAIL); break;
+ case PORT_HGHT_DIAGN: mLCDMenu.vShowScreen(_CANID201FAIL); break;
+ case STARB_HGHT_DIAGN: mLCDMenu.vShowScreen(_CANID202FAIL); break;
+ case GYRO_GPS_DIAGN: mLCDMenu.vShowScreen(_CANID205FAIL); break;
+ case EXT_UI_DIAGN: mLCDMenu.vShowScreen(_CANID1001FAIL); break;
} //end switch
vLCD_update();
} //end if
@@ -578,9 +616,13 @@
// in case system fail
if(bSystemFail){
vCallEmergencyLed();
- mLCDMenu.cShowScreen(_CANNORESPONSE);
- vLCD_update();
- }
+ mLCDMenu.vShowScreen(_CANNORESPONSE);
+ }
+ cMessage = (char) svSoF.iGetHeightFoilBorne();
+ CANbus.write(CANMessage(EXT_UI_HEIGHT, &cMessage, 1));
+
+ mLCDMenu.vSelectMenu(_MENU0_0);
+ vLCD_update();
debug.printf("end can init");
}
@@ -718,17 +760,16 @@
void vSwitchRotated (void)
{
enc_rotated = false;
+
// get value from rotary encoder
thisGet = rSwitch.Get();
- //if(rSwitch.Get()<0)
- //debug.printf("negatief \r\n");
- //else if(rSwitch.Get()>=0)
- //debug.printf("positief \r\n");
- // --- show for development ---
- mLCDMenu.cShowScreen(_PULSE_IS);
+
+ if(rSwitch.Get()<0) mLCDMenu.vRotaryDown();
+ else if(rSwitch.Get()>=0) mLCDMenu.vRotaryUp();
+
+ // reset switch count
+ vResetSwitch();
vLCD_update();
-
- //vLCD_printInt_I2C(thisGet, 1, 11);
}
//////////////////////////////////////////////////////////////////////////////////////
@@ -736,20 +777,14 @@
//////////////////////////////////////////////////////////////////////////////////////
void vSwitchConfirmed (void)
{
- static char cTurns[2] ={0}; //[value][direction]
enc_pressed = false;
- cTurns[0] = 10;//abs(rSwitch.Get()); // send absolute value
- if(rSwitch.Get()>=0) cTurns[1] = CW; // CW
- else cTurns[1] = CCW; // CCW
- CANbus.write(CANMessage(ACTUATOR_STARB_ID, cTurns, 2)); // send message to port actuator
- CANbus.write(CANMessage(ACTUATOR_PORT_ID, cTurns, 2)); // send message to port actuator
+ mLCDMenu.vRotaryConfirm();
+
// reset switch count
vResetSwitch();
- // get value from rotary encoder
- thisGet = rSwitch.Get();
- // --- show for development ---
- //vLCD_printInt_I2C(thisGet, 1, 11);
+
+ vLCD_update();
}
//////////////////////////////////////////////////////////////////////////////////////