takeshi yoshida / Mbed 2 deprecated 2016_lambda_Controller

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

LambdaController.cpp@1:3785af7e715e, 2016-11-12 (annotated)

Committer:
gakuburu
Date:
Sat Nov 12 09:05:29 2016 +0000
Revision:
1:3785af7e715e
Parent:
0:728c138f0486
Child:
2:cf08273cbd94
unko

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gakuburu 0:728c138f0486 1 #include "LambdaController.h"
gakuburu 0:728c138f0486 2
gakuburu 0:728c138f0486 3 LambdaController::LambdaController(PinName tx, PinName rx, PinName hzpin) :
gakuburu 0:728c138f0486 4 device(tx, rx), hzjudge(hzpin)
gakuburu 0:728c138f0486 5 {
gakuburu 0:728c138f0486 6 is_axis_initialized = false;
gakuburu 0:728c138f0486 7
gakuburu 0:728c138f0486 8 buffer_head = -1;
gakuburu 0:728c138f0486 9 device.baud(19200);
gakuburu 0:728c138f0486 10 device.attach(this, &LambdaController::data_receive_isr, Serial::RxIrq);
gakuburu 0:728c138f0486 11
gakuburu 0:728c138f0486 12 hzjudge.mode(PullUp);
gakuburu 0:728c138f0486 13
gakuburu 0:728c138f0486 14 error_count = 0;
gakuburu 0:728c138f0486 15 error_id = NoDataError;
gakuburu 0:728c138f0486 16
gakuburu 0:728c138f0486 17 error_timer.start();
gakuburu 0:728c138f0486 18 }
gakuburu 0:728c138f0486 19
gakuburu 0:728c138f0486 20 void LambdaController::data_receive_isr()
gakuburu 0:728c138f0486 21 {
gakuburu 0:728c138f0486 22 char data, *ptr;
gakuburu 0:728c138f0486 23 int i;
gakuburu 0:728c138f0486 24
gakuburu 0:728c138f0486 25 while (device.readable())
gakuburu 0:728c138f0486 26 {
gakuburu 0:728c138f0486 27 data = device.getc();
gakuburu 0:728c138f0486 28
gakuburu 0:728c138f0486 29 if (data == 0x3A)
gakuburu 0:728c138f0486 30 {
gakuburu 0:728c138f0486 31 buffer_head = 0;
gakuburu 0:728c138f0486 32 }
gakuburu 0:728c138f0486 33
gakuburu 0:728c138f0486 34 if (hzjudge)
gakuburu 0:728c138f0486 35 {
gakuburu 0:728c138f0486 36 if (buffer_head < 21 && buffer_head != -1)
gakuburu 0:728c138f0486 37 {
gakuburu 0:728c138f0486 38 if (data != ',')
gakuburu 0:728c138f0486 39 {
gakuburu 0:728c138f0486 40 if (buffer_head == 0)
gakuburu 0:728c138f0486 41 {
gakuburu 0:728c138f0486 42 received_buffer[buffer_head] = 0x3A;
gakuburu 0:728c138f0486 43 }
gakuburu 0:728c138f0486 44 else
gakuburu 0:728c138f0486 45 {
gakuburu 0:728c138f0486 46 if (buffer_head % 2 == 1)
gakuburu 0:728c138f0486 47 {
gakuburu 0:728c138f0486 48 received_buffer[buffer_head / 2 + 1] = strtol(&data, &ptr, 16) << 4;
gakuburu 0:728c138f0486 49 }
gakuburu 0:728c138f0486 50 else if (buffer_head % 2 == 0)
gakuburu 0:728c138f0486 51 {
gakuburu 0:728c138f0486 52 received_buffer[buffer_head / 2] |= strtol(&data, &ptr, 16);
gakuburu 0:728c138f0486 53 }
gakuburu 0:728c138f0486 54 }
gakuburu 0:728c138f0486 55 buffer_head++;
gakuburu 0:728c138f0486 56 }
gakuburu 0:728c138f0486 57 }
gakuburu 0:728c138f0486 58
gakuburu 0:728c138f0486 59 if (buffer_head == 21)
gakuburu 0:728c138f0486 60 {
gakuburu 0:728c138f0486 61 if (received_buffer[0] == 0x3A)
gakuburu 0:728c138f0486 62 {
gakuburu 0:728c138f0486 63 for (i = 0; i < 11; i++)
gakuburu 0:728c138f0486 64 {
gakuburu 0:728c138f0486 65 received_data[i] = received_buffer[i];
gakuburu 0:728c138f0486 66 }
gakuburu 0:728c138f0486 67
gakuburu 0:728c138f0486 68 if (!is_axis_initialized)
gakuburu 0:728c138f0486 69 {
gakuburu 0:728c138f0486 70 initialize_axis();
gakuburu 0:728c138f0486 71 is_axis_initialized = true;
gakuburu 0:728c138f0486 72 }
gakuburu 0:728c138f0486 73
gakuburu 0:728c138f0486 74 error_count = 0;
gakuburu 0:728c138f0486 75 error_id = NoError;
gakuburu 0:728c138f0486 76 error_timer.reset();
gakuburu 0:728c138f0486 77 }
gakuburu 0:728c138f0486 78 else
gakuburu 0:728c138f0486 79 {
gakuburu 0:728c138f0486 80 buffer_head = -1;
gakuburu 0:728c138f0486 81 error_id = InvalidDataError;
gakuburu 0:728c138f0486 82 }
gakuburu 0:728c138f0486 83 }
gakuburu 0:728c138f0486 84 }
gakuburu 0:728c138f0486 85 else
gakuburu 0:728c138f0486 86 {
gakuburu 0:728c138f0486 87 if (buffer_head < 11 && buffer_head != -1)
gakuburu 0:728c138f0486 88 {
gakuburu 0:728c138f0486 89 received_buffer[buffer_head] = data;
gakuburu 0:728c138f0486 90 buffer_head++;
gakuburu 0:728c138f0486 91 }
gakuburu 0:728c138f0486 92 else
gakuburu 0:728c138f0486 93 {
gakuburu 1:3785af7e715e 94 //error_id = InvalidDataError;
gakuburu 1:3785af7e715e 95 if(buffer_head == -1)
gakuburu 1:3785af7e715e 96 error_id = (ErrorFactor)10;
gakuburu 1:3785af7e715e 97 else if(buffer_head == 11)
gakuburu 1:3785af7e715e 98 error_id = (ErrorFactor)(data + 1);
gakuburu 1:3785af7e715e 99 else
gakuburu 1:3785af7e715e 100 error_id = (ErrorFactor)12;
gakuburu 0:728c138f0486 101 buffer_head = -1;
gakuburu 0:728c138f0486 102 }
gakuburu 0:728c138f0486 103
gakuburu 0:728c138f0486 104 if (buffer_head == 11)
gakuburu 0:728c138f0486 105 {
gakuburu 0:728c138f0486 106 if(received_buffer[0] == 0x3A)
gakuburu 0:728c138f0486 107 {
gakuburu 0:728c138f0486 108 for (i = 0; i < 11; i++)
gakuburu 0:728c138f0486 109 {
gakuburu 0:728c138f0486 110 received_data[i] = received_buffer[i];
gakuburu 0:728c138f0486 111 }
gakuburu 0:728c138f0486 112
gakuburu 0:728c138f0486 113 if (!is_axis_initialized)
gakuburu 0:728c138f0486 114 {
gakuburu 0:728c138f0486 115 initialize_axis();
gakuburu 0:728c138f0486 116 is_axis_initialized = true;
gakuburu 0:728c138f0486 117 }
gakuburu 0:728c138f0486 118
gakuburu 0:728c138f0486 119 error_count = 0;
gakuburu 0:728c138f0486 120 error_id = NoError;
gakuburu 0:728c138f0486 121 error_timer.reset();
gakuburu 0:728c138f0486 122 }
gakuburu 0:728c138f0486 123 else
gakuburu 0:728c138f0486 124 {
gakuburu 0:728c138f0486 125 buffer_head = -1;
gakuburu 0:728c138f0486 126 error_id = InvalidDataError;
gakuburu 0:728c138f0486 127 }
gakuburu 0:728c138f0486 128 }
gakuburu 0:728c138f0486 129 }
gakuburu 0:728c138f0486 130
gakuburu 0:728c138f0486 131 if(data == EOF)
gakuburu 0:728c138f0486 132 {
gakuburu 0:728c138f0486 133 error_id = SerialBusyError;
gakuburu 0:728c138f0486 134 break;
gakuburu 0:728c138f0486 135 }
gakuburu 0:728c138f0486 136 }
gakuburu 0:728c138f0486 137 }
gakuburu 0:728c138f0486 138
gakuburu 0:728c138f0486 139 uint16_t LambdaController::get_all_switch()
gakuburu 0:728c138f0486 140 {
gakuburu 0:728c138f0486 141 return (received_data[1] << 8) | received_data[2];
gakuburu 0:728c138f0486 142 }
gakuburu 0:728c138f0486 143
gakuburu 0:728c138f0486 144 bool LambdaController::get_switch(int num)
gakuburu 0:728c138f0486 145 {
gakuburu 0:728c138f0486 146 bool status = false;
gakuburu 0:728c138f0486 147 if (num < 11 && num >= 0)
gakuburu 0:728c138f0486 148 {
gakuburu 0:728c138f0486 149 if (get_all_switch() & (1 << num))
gakuburu 0:728c138f0486 150 {
gakuburu 0:728c138f0486 151 status = true;
gakuburu 0:728c138f0486 152 }
gakuburu 0:728c138f0486 153 else
gakuburu 0:728c138f0486 154 {
gakuburu 0:728c138f0486 155 status = false;
gakuburu 0:728c138f0486 156 }
gakuburu 0:728c138f0486 157 }
gakuburu 0:728c138f0486 158 else
gakuburu 0:728c138f0486 159 {
gakuburu 0:728c138f0486 160 status = false;
gakuburu 0:728c138f0486 161 }
gakuburu 0:728c138f0486 162
gakuburu 0:728c138f0486 163 return status;
gakuburu 0:728c138f0486 164 }
gakuburu 0:728c138f0486 165
gakuburu 0:728c138f0486 166 int LambdaController::get_raw_axis(AxisId id)
gakuburu 0:728c138f0486 167 {
gakuburu 0:728c138f0486 168 uint8_t *data;
gakuburu 0:728c138f0486 169 int value;
gakuburu 0:728c138f0486 170
gakuburu 0:728c138f0486 171 if((int)id < 4 && (int)id >= 0)
gakuburu 0:728c138f0486 172 {
gakuburu 0:728c138f0486 173 data = &received_data[(int) id * 2 + 3];
gakuburu 0:728c138f0486 174 value = (data[0] << 8) | (data[1]);
gakuburu 0:728c138f0486 175 }
gakuburu 0:728c138f0486 176 else
gakuburu 0:728c138f0486 177 {
gakuburu 0:728c138f0486 178 value = -1;
gakuburu 0:728c138f0486 179 }
gakuburu 0:728c138f0486 180
gakuburu 0:728c138f0486 181 return value;
gakuburu 0:728c138f0486 182 }
gakuburu 0:728c138f0486 183
gakuburu 0:728c138f0486 184 double LambdaController::get_axis(AxisId id)
gakuburu 0:728c138f0486 185 {
gakuburu 0:728c138f0486 186 double value = 0;
gakuburu 0:728c138f0486 187 int raw;
gakuburu 0:728c138f0486 188 raw = get_raw_axis(id);
gakuburu 0:728c138f0486 189
gakuburu 0:728c138f0486 190 if(raw != -1)
gakuburu 0:728c138f0486 191 {
gakuburu 0:728c138f0486 192 value = (double) (raw - axis_center[(int) id]) / 512.0;
gakuburu 0:728c138f0486 193
gakuburu 0:728c138f0486 194 if (value > -axis_threshold && value < axis_threshold)
gakuburu 0:728c138f0486 195 {
gakuburu 0:728c138f0486 196 value = 0.0;
gakuburu 0:728c138f0486 197 }
gakuburu 0:728c138f0486 198 else if (value < -1.0)
gakuburu 0:728c138f0486 199 {
gakuburu 0:728c138f0486 200 value = -1.0;
gakuburu 0:728c138f0486 201 }
gakuburu 0:728c138f0486 202 else if (value > 1.0)
gakuburu 0:728c138f0486 203 {
gakuburu 0:728c138f0486 204 value = 1;
gakuburu 0:728c138f0486 205 }
gakuburu 0:728c138f0486 206 }
gakuburu 0:728c138f0486 207 else
gakuburu 0:728c138f0486 208 {
gakuburu 0:728c138f0486 209 value = 0.0;
gakuburu 0:728c138f0486 210 }
gakuburu 0:728c138f0486 211
gakuburu 0:728c138f0486 212 return value;
gakuburu 0:728c138f0486 213 }
gakuburu 0:728c138f0486 214
gakuburu 0:728c138f0486 215 void LambdaController::initialize_axis(void)
gakuburu 0:728c138f0486 216 {
gakuburu 0:728c138f0486 217 int i;
gakuburu 0:728c138f0486 218 for (i = 0; i < 4; i++)
gakuburu 0:728c138f0486 219 {
gakuburu 0:728c138f0486 220 initialize_axis((AxisId) i);
gakuburu 0:728c138f0486 221 }
gakuburu 0:728c138f0486 222 }
gakuburu 0:728c138f0486 223
gakuburu 0:728c138f0486 224 void LambdaController::initialize_axis(AxisId id)
gakuburu 0:728c138f0486 225 {
gakuburu 0:728c138f0486 226 int raw;
gakuburu 0:728c138f0486 227 raw = get_raw_axis(id);
gakuburu 0:728c138f0486 228
gakuburu 0:728c138f0486 229 if(raw != -1)
gakuburu 0:728c138f0486 230 {
gakuburu 0:728c138f0486 231 axis_center[(int) id] = raw;
gakuburu 0:728c138f0486 232 }
gakuburu 0:728c138f0486 233 }
gakuburu 0:728c138f0486 234
gakuburu 0:728c138f0486 235 LambdaController::ErrorFactor LambdaController::get_error()
gakuburu 0:728c138f0486 236 {
gakuburu 0:728c138f0486 237 if (error_timer >= timeout_time)
gakuburu 0:728c138f0486 238 {
gakuburu 0:728c138f0486 239 error_id = TimeoutError;
gakuburu 0:728c138f0486 240 }
gakuburu 0:728c138f0486 241
gakuburu 0:728c138f0486 242 return error_id;
gakuburu 0:728c138f0486 243 }