Kristof T'Jonck / Mbed 2 deprecated proj2_car

m3pi for proj2

Dependencies:   RemoteIR m3pi mbed-rtos mbed

LightCommunication.cpp@12:f55b31b5bc4a, 2017-05-17 (annotated)

Committer:
gimohd
Date:
Wed May 17 07:14:01 2017 +0000
Revision:
12:f55b31b5bc4a
Parent:
11:e9faca228d30 
changed some things

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 10:7eaaa891ab81 7
Kristof@LAPTOP-FT09DA9V 10:7eaaa891ab81 8
Kristof@LAPTOP-FT09DA9V 10:7eaaa891ab81 9 LightCommunication::LightCommunication()
Kristof@LAPTOP-FT09DA9V 10:7eaaa891ab81 10 {
gimohd 12:f55b31b5bc4a 11 left= 0;
gimohd 12:f55b31b5bc4a 12 right = 0;
Kristof@LAPTOP-FT09DA9V 9:8c5229dfab82 13 ir_rx = new ReceiverIR(p21);
Kristof@LAPTOP-FT09DA9V 10:7eaaa891ab81 14 device = new Serial(p13,p14);
Kristof@LAPTOP-FT09DA9V 9:8c5229dfab82 15 mut = new Mutex();
gimohd 12:f55b31b5bc4a 16 device->baud(9600);
gimohd 12:f55b31b5bc4a 17 //device->attach(callback(this,&LightCommunication::receiveData), Serial::RxIrq);
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 18 }
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 19
Kristof@LAPTOP-FT09DA9V 10:7eaaa891ab81 20 LightCommunication::~LightCommunication()
Kristof@LAPTOP-FT09DA9V 10:7eaaa891ab81 21 {
Kristof@LAPTOP-FT09DA9V 10:7eaaa891ab81 22 delete mut;
Kristof@LAPTOP-FT09DA9V 10:7eaaa891ab81 23 delete device;
Kristof@LAPTOP-FT09DA9V 10:7eaaa891ab81 24 delete ir_rx;
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 25 }
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 26
Kristof@LAPTOP-FT09DA9V 10:7eaaa891ab81 27 void LightCommunication::getIRStyle(uint8_t * buf)
Kristof@LAPTOP-FT09DA9V 10:7eaaa891ab81 28 {
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 29 RemoteIR::Format format;
Kristof@LAPTOP-FT09DA9V 9:8c5229dfab82 30 if (ir_rx->getState() == ReceiverIR::Received) {
Kristof@LAPTOP-FT09DA9V 9:8c5229dfab82 31 ir_rx->getData(&format, buf, sizeof(buf) * 8);
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 32 }
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 33 }
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 34
Kristof@LAPTOP-FT09DA9V 10:7eaaa891ab81 35 void LightCommunication::receiveData()
Kristof@LAPTOP-FT09DA9V 10:7eaaa891ab81 36 {
gimohd 12:f55b31b5bc4a 37 uint8_t data [4];
Kristof@LAPTOP-FT09DA9V 10:7eaaa891ab81 38 //SERIAL RECEIVE
gimohd 12:f55b31b5bc4a 39 //if(device->scanf("%c%c%c",data[0],data[1],data[2])>0) {
gimohd 12:f55b31b5bc4a 40 if (device->readable()){
gimohd 12:f55b31b5bc4a 41 data[0] = device->getc();
gimohd 12:f55b31b5bc4a 42
gimohd 12:f55b31b5bc4a 43
gimohd 12:f55b31b5bc4a 44 printf("DATA");
gimohd 12:f55b31b5bc4a 45 printf("Received data: client:%d \r\n",data[0]);
Kristof@LAPTOP-FT09DA9V 10:7eaaa891ab81 46 if (OWN_ID == (int) data[0]) {
gimohd 12:f55b31b5bc4a 47 data[1] = device->getc();
gimohd 12:f55b31b5bc4a 48 data[2] = device->getc();
gimohd 12:f55b31b5bc4a 49 data[3] = device->getc();
gimohd 12:f55b31b5bc4a 50 printf("%X%X%X%X",data[0],data[1],data[2],data[3]);
gimohd 12:f55b31b5bc4a 51 if(data[3] == (((data[1] + data[2] + 127) % 251) & 0xFF)){
Kristof@LAPTOP-FT09DA9V 10:7eaaa891ab81 52 mut->lock();
Kristof@LAPTOP-FT09DA9V 10:7eaaa891ab81 53 left = LightCommunication::toFloat(data[1]);
Kristof@LAPTOP-FT09DA9V 10:7eaaa891ab81 54 right = LightCommunication::toFloat(data[2]);
Kristof@LAPTOP-FT09DA9V 6:02f045d7695d 55
Kristof@LAPTOP-FT09DA9V 10:7eaaa891ab81 56 printf("DATA left = %f right = %f \r\n",left,right);
Kristof@LAPTOP-FT09DA9V 10:7eaaa891ab81 57 mut->unlock();
gimohd 12:f55b31b5bc4a 58 }
Kristof@LAPTOP-FT09DA9V 10:7eaaa891ab81 59 }
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 60 }
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 61 }
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 62
Kristof@LAPTOP-FT09DA9V 10:7eaaa891ab81 63 float LightCommunication::getRight(float cur_right)
Kristof@LAPTOP-FT09DA9V 10:7eaaa891ab81 64 {
gimohd 12:f55b31b5bc4a 65 float r = right;
gimohd 12:f55b31b5bc4a 66 mut->unlock();
gimohd 12:f55b31b5bc4a 67 return r;
gimohd 12:f55b31b5bc4a 68 /*
Kristof@LAPTOP-FT09DA9V 11:e9faca228d30 69 float r = right - cur_right;
Kristof@LAPTOP-FT09DA9V 11:e9faca228d30 70
Kristof@LAPTOP-FT09DA9V 11:e9faca228d30 71 float cur_speed = 0;
gimohd 12:f55b31b5bc4a 72 if (r>0.0f) {
Kristof@LAPTOP-FT09DA9V 11:e9faca228d30 73 cur_speed = (float)(cur_right + (0.005*SPEED));
gimohd 12:f55b31b5bc4a 74 } else if (r<0.0f) {
Kristof@LAPTOP-FT09DA9V 10:7eaaa891ab81 75
Kristof@LAPTOP-FT09DA9V 11:e9faca228d30 76 cur_speed = (float)(cur_right - (0.005*SPEED));
Kristof@LAPTOP-FT09DA9V 10:7eaaa891ab81 77 } else {
gimohd 12:f55b31b5bc4a 78 cur_speed = cur_right;
Kristof@LAPTOP-FT09DA9V 11:e9faca228d30 79 }
Kristof@LAPTOP-FT09DA9V 11:e9faca228d30 80 mut->unlock();
Kristof@LAPTOP-FT09DA9V 11:e9faca228d30 81 return cur_speed;
gimohd 12:f55b31b5bc4a 82 */
Kristof@LAPTOP-FT09DA9V 10:7eaaa891ab81 83 }
Kristof@LAPTOP-FT09DA9V 10:7eaaa891ab81 84
Kristof@LAPTOP-FT09DA9V 10:7eaaa891ab81 85 float LightCommunication::getLeft(float cur_left)
Kristof@LAPTOP-FT09DA9V 10:7eaaa891ab81 86 {
gimohd 12:f55b31b5bc4a 87 mut->lock();
gimohd 12:f55b31b5bc4a 88 float l = left;
gimohd 12:f55b31b5bc4a 89
gimohd 12:f55b31b5bc4a 90 return l;
gimohd 12:f55b31b5bc4a 91 /*
Kristof@LAPTOP-FT09DA9V 9:8c5229dfab82 92 mut->lock();
Kristof@LAPTOP-FT09DA9V 11:e9faca228d30 93 float l = left - cur_left;
gimohd 12:f55b31b5bc4a 94 if (l>0.0f) {
gimohd 12:f55b31b5bc4a 95 printf("lefdif: %f\r\n",l);
Kristof@LAPTOP-FT09DA9V 10:7eaaa891ab81 96 return (float)(cur_left + (0.005*SPEED));
gimohd 12:f55b31b5bc4a 97 } else if (l<0.0f) {
Kristof@LAPTOP-FT09DA9V 10:7eaaa891ab81 98 return (float)(cur_left - (0.005*SPEED));
Kristof@LAPTOP-FT09DA9V 10:7eaaa891ab81 99 } else {
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 100 return cur_left;
gimohd 12:f55b31b5bc4a 101 }*/
gimohd 12:f55b31b5bc4a 102
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 103 }
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 104
Kristof@LAPTOP-FT09DA9V 10:7eaaa891ab81 105 float LightCommunication::toFloat(int8_t a)
Kristof@LAPTOP-FT09DA9V 10:7eaaa891ab81 106 {
Kristof@LAPTOP-FT09DA9V 11:e9faca228d30 107 return (((float)a)/100.0f)*SPEED;
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 108 }
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 109
Kristof@LAPTOP-FT09DA9V 10:7eaaa891ab81 110 bool LightCommunication::needsToStop()
Kristof@LAPTOP-FT09DA9V 10:7eaaa891ab81 111 {
Kristof@LAPTOP-FT09DA9V 10:7eaaa891ab81 112 mut->lock();
Kristof@LAPTOP-FT09DA9V 10:7eaaa891ab81 113 bool stop = right == 0 && left == 0;
Kristof@LAPTOP-FT09DA9V 10:7eaaa891ab81 114 mut->unlock();
Kristof@LAPTOP-FT09DA9V 10:7eaaa891ab81 115 return stop;
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 116 }
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 117
Kristof@LAPTOP-FT09DA9V.home 1:76cff32344d1 118