version 1.0
Dependencies: CMSIS_DSP_401 GPS MPU9150_DMP PID QuaternionMath Servo mbed
Fork of SolarOnFoils_MainModule_20150518 by
Diff: main.cpp
- Revision:
- 0:81b21910454e
- Child:
- 1:b4a0d63db637
diff -r 000000000000 -r 81b21910454e main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Tue Jun 23 13:55:28 2015 +0000 @@ -0,0 +1,828 @@ +////////////////////////////////////////////////////////////////////////////////////// +// // +// File : main.cpp // +// Version : 0.1 // +// Date : 25 march 2015 // +// Author : Dany Brugman // +// Comment : Function to write data to a 2x16 LCD by I2C // +// using a MCP23017 port expander. // +// // +// Changelog : // +// Date: Name: Comment: // +// 25/03/2015 DNB First version // +// // +////////////////////////////////////////////////////////////////////////////////////// + +////////////////////////////////////////////////////////////////////////////////////// +// includes // +////////////////////////////////////////////////////////////////////////////////////// + +#include "mbed.h" +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include "MCP23017.h" +#include "LCD_I2C.h" +#include "uart.h" +#include "GPS.h" +#include "MPU9150.h" +#include "Quaternion.h" +#include "PID.h" +#include "Servo.h" +#include "mRotaryEncoder.h" +#include "MainModule.h" +#include "LCD_Menu.h" +#include "menu.h" +#include "systemVar.h" + +////////////////////////////////////////////////////////////////////////////////////// +// defines // +////////////////////////////////////////////////////////////////////////////////////// + +#define CLEAR "\033[2J" +#define PROJECT "\033[1;10fSolar on Foils\n" +#define VERSION "\033[2;10fVersion 1.0\n" + +#define ACTUATOR_PORT_ID 101 +#define ACTUATOR_STARB_ID 102 +#define HEIGHT_PORT_ID 201 +#define HEIGHT_STARB_ID 202 +#define GYRO_GPS_ID 205 +#define EXT_UI_ID 20 +#define MESSAGE_ALL 2000 +#define PORT_ACT_DIAGN 2001 +#define STARB_ACT_DIAGN 2002 +#define PORT_HGHT_DIAGN 2003 +#define STARB_HGHT_DIAGN 2004 +#define GYRO_GPS_DIAGN 2005 +#define EXT_UI_DIAGN 2006 + +#define CCW 0 +#define CW 1 + +#define V_MAX 1.85 +#define NSAMPLES 10000 //current samples for ref. + +#define ROLL_IN_MIN -90 +#define ROLL_IN_MAX 90 +#define ROLL_OUT_MIN -0.1 +#define ROLL_OUT_MAX 0.1 + +#define Kc 1.0 +#define Ti 0.0 +#define Td 0.0 +#define RATE 0.01 + +enum MSG_t +{ + MSG1 = 0, // message 1 + MSG2, // message 2 + MSG3 // message 3 +}; + +////////////////////////////////////////////////////////////////////////////////////// +// port declaration // +////////////////////////////////////////////////////////////////////////////////////// + +DigitalOut myLed(LED1); +DigitalOut myLed2(LED2); + +DigitalOut aliveLed(p23); +DigitalOut statusLed(p26); +DigitalOut emergencyLed(p25); +DigitalOut motorRelais(p20); + +AnalogIn currentSensor(p15); + +InterruptIn intAGM(p8); + +I2C i2c(p9, p10); +GPS gps(p13, p14); +// I2C Rx, I2C Tx, Int +MPU9150 imu(p10, p9, p8); +//mRotaryEncoder(PinName pinA, PinName pinB, PinName pinSW, PinMode pullMode=PullUp, int debounceTime_us=1000) +mRotaryEncoder rSwitch(p12, p11, p7); +CAN CANbus(p30, p29); +MCP23017 mcp23017 = MCP23017(i2c, 0x40); +Serial debug(USBTX, USBRX); + +PID rollPID(Kc,Ti,Td,RATE); // declare a PID for roll adjustment + +////////////////////////////////////////////////////////////////////////////////////// +// function prototypes // +////////////////////////////////////////////////////////////////////////////////////// + + +////////////////////////////////////////////////////////////////////////////////////// +// global variables // +////////////////////////////////////////////////////////////////////////////////////// + +int i = 0; +int value = 0; +int lastGet; +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 +bool bSystemFail = false; // sytem fail + +char buffer_GPIOB; +char cBuffer[200]; +char cMessage; +char text[16]; +//char buffer[16]; + +float fRoll, fPitch, fEulerRoll; +float fRollDiff; +float fPreviousTime; + +double dCurrent = 0.0; +double dRefCurrent = 0.0; + +Quaternion q1; + +Ticker CANTicker; +Ticker tAliveOn; +Ticker tEmergencyLedOn; +Ticker tMPU; + +Timeout tAliveOff; +Timeout tEmergencyLedOff; +Timer actualTime; +Timer timer; + +Menu mLCDMenu; +SystemVar svSoF; + +MSG_t messageToSend; +////////////////////////////////////////////////////////////////////////////////////// +// main // +////////////////////////////////////////////////////////////////////////////////////// + +int main() +{ +////////////////////////////////////////////////////////////////////////////////////// +// init // +////////////////////////////////////////////////////////////////////////////////////// + vInit(); + // init CANbus + vInitCANBus(); + // init Gyro + vInitImu(); + + + CANMessage msg; + messageToSend = MSG1; + + + //acquiring v_ref + for (int i=0; i<NSAMPLES; i++) + { + dRefCurrent += currentSensor; + } + dRefCurrent /= NSAMPLES; + //debug.printf("RefCurrent : %f\n\r", dRefCurrent); + + timer.start(); //timer to test imu roll angle + actualTime.start(); //crash timer + // clear line -1- + vLCD_printPos_I2C((unsigned char*)" ", 1, 1); + + // put text on lcd + + //mLCDMenu.cShowScreen(_FOILBORNE); + //vLCD_printPos_I2C((unsigned char*)mLCDMenu.getLine1(), 2, 1); + +////////////////////////////////////////////////////////////////////////////////////// +// endless loop // +////////////////////////////////////////////////////////////////////////////////////// + while (1) + { + // rotary switch has been rotated? + if (enc_rotated) vSwitchRotated(); + + // rotary switch confirm pressed? + if (enc_pressed) vSwitchConfirmed(); + + // answer on id#1003 + if(CANbus.read(msg)){ + 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) + { + timer.reset(); + svSoF.vSetPitch(q1); // call function class SystemVar + mLCDMenu.cShowScreen(_PITCHTEST); + vLCD_update(); + + //debug.printf("Roll: %c\t", svSoF.getValue()); + //debug.printf("Roll: %f\t", getRollAngle(q1)); + //debug.printf("Pitch: %f\t", getPitchAngle(q1)); + //debug.printf("Time: %f\r\n", actualTime.read()); + } + /*for (int samples = 0; samples<NSAMPLES; samples++) dCurrent += currentSensor; + + dCurrent /= NSAMPLES; + //debug.printf("RefCurrent : %f\n\r", dRefCurrent); + //debug.printf("dCurrent : %f\n\r", dCurrent); + dCurrent = (dCurrent-dRefCurrent)*V_MAX/0.185; + + debug.printf("Current mA : %f\n\r", (dCurrent*1000));*/ + wait(0.2); + + } + +} // END MAIN + +////////////////////////////////////////////////////////////////////////////////////// +// delay function for LCD // +////////////////////////////////////////////////////////////////////////////////////// + +void vLCD_delay_I2C (unsigned int t_delay) +{ + unsigned int i; + + for (i=0; i < t_delay; i++); +} + +////////////////////////////////////////////////////////////////////////////////////// +// write commando to LCD // +////////////////////////////////////////////////////////////////////////////////////// + +void vLCD_cmd_I2C (unsigned char commando) +{ +__disable_irq(); // Disable Interrupts + + // write commando to LCD + mcp23017.write(PORT_A, commando); + + // LCD_RS = 0 + buffer_GPIOB = buffer_GPIOB & 0xFD; // RS = 0 + // write to LCD + mcp23017.write(PORT_B, buffer_GPIOB); + + // LCD_EN = 1 + buffer_GPIOB = buffer_GPIOB | 0x01; // EN = 1 + // write to LCD + mcp23017.write(PORT_B, buffer_GPIOB); + + // LCD_EN = 0 + buffer_GPIOB = buffer_GPIOB & 0xFE; // EN = 0 + // write to LCD + mcp23017.write(PORT_B, buffer_GPIOB); + +__enable_irq(); // Enable Interrupts +} + +////////////////////////////////////////////////////////////////////////////////////// +// write single char to LCD // +////////////////////////////////////////////////////////////////////////////////////// + +void vLCD_data_I2C (unsigned char data) +{ +__disable_irq(); // Disable Interrupts + + // write data to LCD + mcp23017.write(PORT_A, data); + + // LCD_RS = 1 + buffer_GPIOB = buffer_GPIOB | 0x02; // RS = 1 + // write to LCD + mcp23017.write(PORT_B, buffer_GPIOB); + + // LCD_EN = 1 + buffer_GPIOB = buffer_GPIOB | 0x01; // EN = 1 + // write to LCD + mcp23017.write(PORT_B, buffer_GPIOB); + + // LCD_EN = 0 + buffer_GPIOB = buffer_GPIOB & 0xFE; // EN = 0 + // write to LCD + mcp23017.write(PORT_B, buffer_GPIOB); + +__enable_irq(); // Enable Interrupts +} + +////////////////////////////////////////////////////////////////////////////////////// +// Initialize LCD // +////////////////////////////////////////////////////////////////////////////////////// + +void vLCD_init_I2C (void) +{ + vLCD_cmd_I2C (0x00); + // vLCD_delay_I2C (65535) + vLCD_delay_I2C (100); + + vLCD_cmd_I2C (0x38); + // vLCD_delay_I2C (65535) + vLCD_delay_I2C (100); + + vLCD_cmd_I2C (0x38); + //vLCD_delay_I2C (65535) + vLCD_delay_I2C (100); + + vLCD_cmd_I2C (0x38); + //vLCD_delay_I2C (65535) + vLCD_delay_I2C (100); + + // Display ON / OFF + vLCD_cmd_I2C (0x08); + + //vLCD_delay_I2C (65535) + vLCD_delay_I2C (100); + + // Clear Display + vLCD_cmd_I2C (0x01); + + //vLCD_delay_I2C (65535) + vLCD_delay_I2C (100); + + // Entry Mode Set + vLCD_cmd_I2C (0x06); + + //vLCD_delay_I2C (65535) + vLCD_delay_I2C (100); + + // Display ON / OFF + vLCD_cmd_I2C (0x0C); + + //vLCD_delay_I2C (65535) + vLCD_delay_I2C (100); +} + +////////////////////////////////////////////////////////////////////////////////////// +// Write string to LCD // +////////////////////////////////////////////////////////////////////////////////////// + +void vLCD_print_I2C (unsigned char *string, unsigned char line) +{ + unsigned int i; + + // select target line to write to + if (line == 1) + { + vLCD_cmd_I2C (0x80); + } + else if (line == 2) + { + vLCD_cmd_I2C (0xC0); + } + else + { + return; // End of function: Error in the past value of line + } + + // write data to selected line + for (i=0; i<16; i++) + { + if (string [i] != 0x00) + { + vLCD_data_I2C (string [i]); + } + else + vLCD_data_I2C (' '); + } +} + +////////////////////////////////////////////////////////////////////////////////////// +// Write string to LCD // +////////////////////////////////////////////////////////////////////////////////////// + +void vLCD_printPos_I2C (unsigned char *string, unsigned char line, unsigned char character) +{ + unsigned int i; + + // select target line and position to write to + if (line == 1) + { + vLCD_cmd_I2C (0x80+(character-1)); + } + + else if (line == 2) + { + vLCD_cmd_I2C (0xC0+(character-1)); + } + + else + { + return; // End of function: Error in the past value of line + } + + // write data to selected line and position + for (i=0; i<16; i++) + { + if (string [i] != 0x00) + { + vLCD_data_I2C (string [i]); + } + else + vLCD_data_I2C (' '); + } +} + +////////////////////////////////////////////////////////////////////////////////////// +// update LCD // +////////////////////////////////////////////////////////////////////////////////////// +void vLCD_clear_I2C (void) +{ + // Clear Display + vLCD_cmd_I2C (0x01); + //vLCD_delay_I2C (65535) + vLCD_delay_I2C (100); + // Display ON / OFF + vLCD_cmd_I2C (0x0C); + //vLCD_delay_I2C (65535) + vLCD_delay_I2C (100); +} + +////////////////////////////////////////////////////////////////////////////////////// +// update LCD // +////////////////////////////////////////////////////////////////////////////////////// +void vLCD_update (void) +{ + vLCD_print_I2C((unsigned char*)mLCDMenu.getLine1(), 1); + vLCD_print_I2C((unsigned char*)mLCDMenu.getLine2(), 2); + //vLCD_printPos_I2C((unsigned char*)mLCDMenu.getLine1(), 1, 1); + //vLCD_printPos_I2C((unsigned char*)mLCDMenu.getLine2(), 2, 1); +} + +////////////////////////////////////////////////////////////////////////////////////// +// Init SOF module // +////////////////////////////////////////////////////////////////////////////////////// +void vInit (void) +{ + // I2C init + i2c.frequency(100000); + //Port A is databus - Output + mcp23017.direction(PORT_A, PORT_DIR_OUT); + //Port B is controlbus - Output + mcp23017.direction(PORT_B, PORT_DIR_OUT); + + debug.baud(115200); + // initialize LCD + vLCD_init_I2C(); + mLCDMenu.cShowScreen(_INIT); + vLCD_update(); + + // led's out + aliveLed = 1; wait(0.1); aliveLed = 0; + statusLed = 1; wait(0.1); statusLed = 0; + emergencyLed = 1; wait(0.1); emergencyLed = 0; + + wait(0.2); + motorRelais = 1; + wait(1.0); + motorRelais = 0; + + //Int-Handler rotary encoder switch + // call trigger_sw() when button of rotary-encoder is pressed + rSwitch.attachSW(&trigger_sw); + // call trigger_rot() when the shaft is rotaded left or right + rSwitch.attachROT(&trigger_rotated); + lastGet = 0; + // set encrotated, so position is displayed on startup + enc_rotated = true; + // blink alive LED + aliveLed = 1; + tAliveOff.attach(&vBlinkOnce_cb, 0.05); + tAliveOn.attach(&vBlinkOnce_cb, 5); +} + +////////////////////////////////////////////////////////////////////////////////////// +// Init MPU 9150 // +////////////////////////////////////////////////////////////////////////////////////// +void vInitImu (void) +{ + if(imu.isReady()) mLCDMenu.cShowScreen(_IMUREADY); + else mLCDMenu.cShowScreen(_IMUFAIL); + vLCD_update(); + + imu.initialiseDMP(); + imu.setFifoReset(true); + imu.setDMPEnabled(true); + intAGM.rise(&vGetMPUBuffer); + //tMPU.attach_us(&vGetMPUBuffer,50000); +} + +////////////////////////////////////////////////////////////////////////////////////// +// Init CANbus // +////////////////////////////////////////////////////////////////////////////////////// +void vInitCANBus (void) +{ + CANbus.frequency(250000); + debug.printf("vInitCANBus\r\n"); + CANMessage msg; + int i, iTry = 10; + uint32_t id = MESSAGE_ALL; + bool bReceived = false; + cMessage = 0x00; + // check CAN system + for (i = 6; i > 0 ; i--) + { + debug.printf("for loop: %i\r\n", i); + bReceived = false; + id++; // increase id + while ((iTry > 0)&&(!bReceived)) + { + CANbus.write(CANMessage(id, &cMessage, 1)); + wait(0.15); + switch(id){ // set adress to filter + case PORT_ACT_DIAGN: uiFilterId = ACTUATOR_PORT_ID; break; + case STARB_ACT_DIAGN: uiFilterId = ACTUATOR_STARB_ID; break; + case PORT_HGHT_DIAGN: uiFilterId = HEIGHT_PORT_ID; break; + case STARB_HGHT_DIAGN: uiFilterId = HEIGHT_STARB_ID; break; + case GYRO_GPS_DIAGN: uiFilterId = GYRO_GPS_ID; break; + case EXT_UI_DIAGN: uiFilterId = EXT_UI_ID; break; + }//end switch + iFilter = CANbus.filter(0x200, 0x7FF, CANStandard); + if(CANbus.read(msg, iFilter)){ + 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; + } //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; + } //end switch + vLCD_update(); + } //end if + if(CANbus.read(msg)){}//unwanted message + if(!bReceived) iTry--; + debug.printf("Tries: %i\r\n", iTry); + } //end while + wait(0.5); // some time to read message + iTry = 10; // reset try counter +// *****afhandelen can fail **** + if (iTry != 0) iTry = 10; + else bSystemFail = true; + } //end for + + // in case system fail + if(bSystemFail){ + vCallEmergencyLed(); + mLCDMenu.cShowScreen(_CANNORESPONSE); + vLCD_update(); + } + debug.printf("end can init"); +} + +// call vCANBusSend every 1 second test +// CANTicker.attach(&vCANBusSend, 1); + +////////////////////////////////////////////////////////////////////////////////////// +// Blink alive led // +////////////////////////////////////////////////////////////////////////////////////// +void vBlinkOff_cb (void) +{ + aliveLed = 0; +} + +////////////////////////////////////////////////////////////////////////////////////// +// Blink emergecy led // +////////////////////////////////////////////////////////////////////////////////////// +void vEmergencyOff_cb (void) +{ + emergencyLed = 0; +} + +////////////////////////////////////////////////////////////////////////////////////// +// Blink status led once // +////////////////////////////////////////////////////////////////////////////////////// +void vBlinkOnce_cb (void) +{ + aliveLed = 1; + tAliveOff.detach(); + tAliveOff.attach(&vBlinkOff_cb, 0.05); +} + +////////////////////////////////////////////////////////////////////////////////////// +// Blink emergency led once // +////////////////////////////////////////////////////////////////////////////////////// +void vEmergencyOnce_cb (void) +{ + emergencyLed = 1; + tEmergencyLedOff.detach(); + tEmergencyLedOff.attach(&vEmergencyOff_cb, 0.05); + + if (!bSystemFail) + { + tEmergencyLedOn.detach(); + tEmergencyLedOff.detach(); + aliveLed = 0; + } +} + +////////////////////////////////////////////////////////////////////////////////////// +// Call emergency led // +////////////////////////////////////////////////////////////////////////////////////// +void vCallEmergencyLed(void) +{ + aliveLed = 1; + tEmergencyLedOff.attach(&vEmergencyOnce_cb, 0.05); + tEmergencyLedOn.attach(&vEmergencyOnce_cb, 0.5); +} + +////////////////////////////////////////////////////////////////////////////////////// +// Writing integer to LCD // +////////////////////////////////////////////////////////////////////////////////////// +void vLCD_printInt_I2C (int value, unsigned char line, unsigned char character) +{ + static char buffer[32]; + // convert int to char + sprintf(buffer, "%.2d", value); + // write converted int to LCD + vLCD_printPos_I2C((unsigned char*)buffer, line, character); +} + +////////////////////////////////////////////////////////////////////////////////////// +// calculate Roll from quaternion // +////////////////////////////////////////////////////////////////////////////////////// +float getRollAngle(Quaternion q1) +{ + float fRoll; + 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); + fRoll *= 180/3.14; + return (fRoll); +} + +////////////////////////////////////////////////////////////////////////////////////// +// calculate Pitch from quaternion // +////////////////////////////////////////////////////////////////////////////////////// +float getPitchAngle(Quaternion q1) +{ + Vector3 euler = q1.getEulerAngles(); + //debug.printf("$ Pitch = %f #\t", euler.y); + float fPitch; + //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))))); + //fPitch = acos(-(q1.w*q1.w) - (q1.v.x*q1.v.x) + (q1.v.y*q1.v.y) + (q1.v.z*q1.v.z)); + //fPitch = -asin(2*q1.w*q1.v.y - 2*q1.v.x*q1.v.z); + fPitch = euler.y; + fPitch *= 180/3.14; + return (fPitch); + +} + +////////////////////////////////////////////////////////////////////////////////////// +// interrup-Handler for button on rotary-encoder // +////////////////////////////////////////////////////////////////////////////////////// +void trigger_sw(void) +{ + enc_pressed = true; // just set the flag, rest is done outside isr +} + +////////////////////////////////////////////////////////////////////////////////////// +// interrup-Handler for rotary-encoder rotation // +////////////////////////////////////////////////////////////////////////////////////// +void trigger_rotated(void) +{ + enc_rotated = true; // just set the flag, rest is done outside isr +} + +////////////////////////////////////////////////////////////////////////////////////// +// reset for rotary-encoder switch // +////////////////////////////////////////////////////////////////////////////////////// +void vResetSwitch (void) +{ + rSwitch.Set(0); +} + +////////////////////////////////////////////////////////////////////////////////////// +// set for rotary-encoder switch // +////////////////////////////////////////////////////////////////////////////////////// +void vSetSwitch (int value) +{ + rSwitch.Set(value); +} + +////////////////////////////////////////////////////////////////////////////////////// +// rotary-encoder switch rotated // +////////////////////////////////////////////////////////////////////////////////////// +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); + vLCD_update(); + + //vLCD_printInt_I2C(thisGet, 1, 11); +} + +////////////////////////////////////////////////////////////////////////////////////// +// rotary-encoder switch confirmed // +////////////////////////////////////////////////////////////////////////////////////// +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 + // reset switch count + vResetSwitch(); + // get value from rotary encoder + thisGet = rSwitch.Get(); + // --- show for development --- + //vLCD_printInt_I2C(thisGet, 1, 11); +} + +////////////////////////////////////////////////////////////////////////////////////// +// CANbus send not used // +////////////////////////////////////////////////////////////////////////////////////// +void vCANBusSend(void) +{ + static char counter = 0; + static char counterTwo = 0; + static char random[8] = {0}; + //vLCD_printPos_I2C((unsigned char*)"CANbus send ", 2, 1); + switch(messageToSend){ + case MSG1: + if(CANbus.write(CANMessage(1336, &counter, 1))) + { + counter++; + messageToSend = MSG2; + break; + } + case MSG2: + if(CANbus.write(CANMessage(1003, &counterTwo, 1))) + { + counterTwo++; + messageToSend = MSG3; + break; + } + case MSG3: + if(CANbus.write(CANMessage(1236, random, 4))) + { + random[0] = rand()% 10 + 1; + random[1] = rand()% 10 + 20; + random[2] = rand()% 100 + 1; + random[3] = 00; + //messageToSend = MSG1; + break; + } + } // end switch case + + myLed = !myLed; +} + +////////////////////////////////////////////////////////////////////////////////////// +// Get data from MPU fifo buffer // +////////////////////////////////////////////////////////////////////////////////////// +void vGetMPUBuffer(void) +{ + int iFifoCount; + if (imu.getInterruptFifoOverflow()) + imu.setFifoReset(true); + iFifoCount = imu.getFifoCount(); + //debug.printf("fifocount: %i\r\n", iFifoCount); + if(imu.getFifoCount() >= 48){ + if(iFifoCount % 48 != 0) imu.getFifoBuffer(cBuffer, iFifoCount % 48); + //imu.getFifoBuffer(cBuffer, iFifoCount- 48); + imu.getFifoBuffer(cBuffer, 48); + //myLed2 = !myLed2; + q1.decode(cBuffer);// quaternion vector to buffer + } +} + +////////////////////////////////////////////////////////////////////////////////////// +// EOF // +////////////////////////////////////////////////////////////////////////////////////// + + /*if (fifoCount == 1024) // test + {mpu.resetFIFO();} // reset so we can continue cleanly + + else if (fifoCount >= 42) // otherwise, check for DMP data ready interrupt (this should happen frequently) + { + if ((fifoCount % stMPU9150->packetSize) != 0) // test + {mpu.getFIFOBytes(fifoBuffer, (fifoCount % stMPU9150->packetSize));} // test + + while (mpu.getFIFOCount() >= 42) + {mpu.getFIFOBytes(fifoBuffer, stMPU9150->packetSize);} // read a packet from FIFO + + mpu.dmpGetQuaternion(&stMPU9150->q, fifoBuffer);*/