Kristof T'Jonck / Mbed 2 deprecated proj2_car

m3pi for proj2

Dependencies:   RemoteIR m3pi mbed-rtos mbed

LightCommunication.cpp@1:76cff32344d1, 2017-05-09 (annotated)

Committer:
Kristof@LAPTOP-FT09DA9V.home
Date:
Tue May 09 23:02:13 2017 +0200
Revision:
1:76cff32344d1
Child:
2:70240992e869
added communication

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 1 //
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 2 // Created by Kristof on 5/9/2017.
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 3 //
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 4
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 5 #include "LightCommunication.h"
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 6
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 7 LightCommunication::LightCommunication() {
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 8 left,right = 0;
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 9 device = new Serial(p14,p15);
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 10 ir_rx = new ReceiverIR(p21);
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 11 }
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 12
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 13 LightCommunication::~LightCommunication() {
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 14
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 15 }
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 16
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 17 char * LightCommunication::getSerial() {
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 18 char test[3];
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 19 if(device->readable()) {
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 20 device->scanf("%s",test);
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 21 }
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 22 return test;
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 23 }
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 24
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 25 uint8_t * LightCommunication::getIRStyle() {
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 26 RemoteIR::Format format;
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 27 uint8_t buf[32];
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 28 int bitcount;
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 29 if (ir_rx->getState() == ReceiverIR::Received) {
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 30 bitcount = ir_rx->getData(&format, buf, sizeof(buf) * 8);
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 31 }
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 32 return buf;
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 33 }
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 34
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 35 void LightCommunication::receiveData() {
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 36 mut.lock();
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 37 //uint8_t * data = LightCommunication::getIRStyle();
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 38 char * data = LightCommunication::getSerial();
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 39
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 40 printf("%X \n",data[2]);
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 41 printf("%d \n",data[2]);
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 42 if (OWN_ID == (int) data[0]) {
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 43 left = LightCommunication::toFloat(data[1]);
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 44 right = LightCommunication::toFloat(data[2]);
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 45 }
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 46 mut.unlock();
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 47 }
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 48
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 49 float LightCommunication::getRight(float cur_right) const {
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 50 float r = right - cur_right;
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 51 if (r>0){
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 52 return (cur_right + 0.005);
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 53 }else if (r<0){
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 54
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 55 return (cur_right -0.005);
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 56 }else{
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 57 return cur_right;
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 58 }
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 59 }
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 60
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 61 float LightCommunication::getLeft(float cur_left) const {
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 62
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 63 mut.lock();
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 64 float l = left - cur_left;
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 65 if (l>0){
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 66 return (cur_left + 0.005);
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 67 }else if (l<0){
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 68 return (cur_left -0.005);
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 69 }else{
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 70 return cur_left;
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 71 }
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 72 }
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 73
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 74 float LightCommunication::toFloat(int8_t a) {
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 75 return ((float)a)/100;
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 76 }
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 77
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 78 bool LightCommunication::needsToStop() {
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 79 if (right == 0 && left == 0){
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 80 return true;
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 81 }
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 82 return false;
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 83 }
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 84
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 85