Yi Lin Chen
shared
Revision 8:089b778962c4, committed 2014-10-31
- Comitter:
- Kruskal
- Date:
- Fri Oct 31 10:54:51 2014 +0000
- Parent:
- 7:fe8665daf3e7
- Commit message:
- :D
Changed in this revision
diff -r fe8665daf3e7 -r 089b778962c4 TSISensor.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/TSISensor.cpp Fri Oct 31 10:54:51 2014 +0000
@@ -0,0 +1,227 @@
+/* Freescale Semiconductor Inc.
+ * (c) Copyright 2004-2005 Freescale Semiconductor, Inc.
+ * (c) Copyright 2001-2004 Motorola, Inc.
+ *
+ * mbed Microcontroller Library
+ * (c) Copyright 2009-2012 ARM Limited.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
+ * and associated documentation files (the "Software"), to deal in the Software without
+ * restriction, including without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all copies or
+ * substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
+ * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
+ * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#include "mbed.h"
+#include "TSISensor.h"
+
+#define NO_TOUCH 0
+#define SLIDER_LENGTH 40 //LENGTH in mm
+#define TOTAL_ELECTRODE 2
+
+#define TSI0a 0
+#define TSI1 1
+#define TSI2 2
+#define TSI3 3
+#define TSI4 4
+#define TSI5 5
+#define TSI6 6
+#define TSI7 7
+#define TSI8 8
+#define TSI9 9
+#define TSI10 10
+#define TSI11 11
+#define TSI12 12
+#define TSI13 13
+#define TSI14 14
+#define TSI15 15
+
+/*Chose the correct TSI channel for the electrode number*/
+#define ELECTRODE0 TSI9
+#define ELECTRODE1 TSI10
+#define ELECTRODE2 TSI0a
+#define ELECTRODE3 TSI1
+#define ELECTRODE4 TSI2
+#define ELECTRODE5 TSI3
+#define ELECTRODE6 TSI4
+#define ELECTRODE7 TSI5
+#define ELECTRODE8 TSI6
+#define ELECTRODE9 TSI7
+#define ELECTRODE10 TSI8
+#define ELECTRODE11 TSI11
+#define ELECTRODE12 TSI12
+#define ELECTRODE13 TSI13
+#define ELECTRODE14 TSI14
+#define ELECTRODE15 TSI15
+
+#define THRESHOLD0 100
+#define THRESHOLD1 100
+#define THRESHOLD2 100
+#define THRESHOLD3 100
+#define THRESHOLD4 100
+#define THRESHOLD5 100
+#define THRESHOLD6 100
+#define THRESHOLD7 100
+#define THRESHOLD8 100
+#define THRESHOLD9 100
+#define THRESHOLD10 100
+#define THRESHOLD11 100
+#define THRESHOLD12 100
+#define THRESHOLD13 100
+#define THRESHOLD14 100
+#define THRESHOLD15 100
+
+static uint8_t total_electrode = TOTAL_ELECTRODE;
+static uint8_t elec_array[16]={ELECTRODE0,ELECTRODE1,ELECTRODE2,ELECTRODE3,ELECTRODE4,ELECTRODE5,
+ ELECTRODE6,ELECTRODE7,ELECTRODE8,ELECTRODE9,ELECTRODE10,ELECTRODE11,
+ ELECTRODE12,ELECTRODE13,ELECTRODE14,ELECTRODE15};
+static uint16_t gu16TSICount[16];
+static uint16_t gu16Baseline[16];
+static uint16_t gu16Threshold[16]={THRESHOLD0,THRESHOLD1,THRESHOLD2,THRESHOLD3,THRESHOLD4,THRESHOLD5,
+ THRESHOLD6,THRESHOLD7,THRESHOLD8,THRESHOLD9,THRESHOLD10,THRESHOLD11,
+ THRESHOLD12,THRESHOLD13,THRESHOLD14,THRESHOLD15};
+static uint16_t gu16Delta[16];
+static uint8_t ongoing_elec;
+static uint8_t end_flag = 1;
+
+static uint8_t SliderPercentegePosition[2] = {NO_TOUCH,NO_TOUCH};
+static uint8_t SliderDistancePosition[2] = {NO_TOUCH,NO_TOUCH};
+static uint32_t AbsolutePercentegePosition = NO_TOUCH;
+static uint32_t AbsoluteDistancePosition = NO_TOUCH;
+
+static void tsi_irq();
+
+TSISensor::TSISensor() {
+ SIM->SCGC5 |= SIM_SCGC5_PORTB_MASK;
+ SIM->SCGC5 |= SIM_SCGC5_TSI_MASK;
+
+ TSI0->GENCS |= (TSI_GENCS_ESOR_MASK
+ | TSI_GENCS_MODE(0)
+ | TSI_GENCS_REFCHRG(4)
+ | TSI_GENCS_DVOLT(0)
+ | TSI_GENCS_EXTCHRG(7)
+ | TSI_GENCS_PS(4)
+ | TSI_GENCS_NSCN(11)
+ | TSI_GENCS_TSIIEN_MASK
+ | TSI_GENCS_STPE_MASK
+ );
+
+ TSI0->GENCS |= TSI_GENCS_TSIEN_MASK;
+
+ NVIC_SetVector(TSI0_IRQn, (uint32_t)&tsi_irq);
+ NVIC_EnableIRQ(TSI0_IRQn);
+
+ selfCalibration();
+}
+
+void TSISensor::selfCalibration(void)
+{
+ unsigned char cnt;
+ unsigned char trigger_backup;
+
+ TSI0->GENCS |= TSI_GENCS_EOSF_MASK; // Clear End of Scan Flag
+ TSI0->GENCS &= ~TSI_GENCS_TSIEN_MASK; // Disable TSI module
+
+ if(TSI0->GENCS & TSI_GENCS_STM_MASK) // Back-up TSI Trigger mode from Application
+ trigger_backup = 1;
+ else
+ trigger_backup = 0;
+
+ TSI0->GENCS &= ~TSI_GENCS_STM_MASK; // Use SW trigger
+ TSI0->GENCS &= ~TSI_GENCS_TSIIEN_MASK; // Enable TSI interrupts
+
+ TSI0->GENCS |= TSI_GENCS_TSIEN_MASK; // Enable TSI module
+
+ for(cnt=0; cnt < total_electrode; cnt++) // Get Counts when Electrode not pressed
+ {
+ TSI0->DATA = ((elec_array[cnt] << TSI_DATA_TSICH_SHIFT) );
+ TSI0->DATA |= TSI_DATA_SWTS_MASK;
+ while(!(TSI0->GENCS & TSI_GENCS_EOSF_MASK));
+ TSI0->GENCS |= TSI_GENCS_EOSF_MASK;
+ gu16Baseline[cnt] = (TSI0->DATA & TSI_DATA_TSICNT_MASK);
+ }
+
+ TSI0->GENCS &= ~TSI_GENCS_TSIEN_MASK; // Disable TSI module
+ TSI0->GENCS |= TSI_GENCS_TSIIEN_MASK; // Enale TSI interrupt
+ if(trigger_backup) // Restore trigger mode
+ TSI0->GENCS |= TSI_GENCS_STM_MASK;
+ else
+ TSI0->GENCS &= ~TSI_GENCS_STM_MASK;
+
+ TSI0->GENCS |= TSI_GENCS_TSIEN_MASK; // Enable TSI module
+
+ TSI0->DATA = ((elec_array[0]<<TSI_DATA_TSICH_SHIFT) );
+ TSI0->DATA |= TSI_DATA_SWTS_MASK;
+}
+
+void TSISensor::sliderRead(void ) {
+ if(end_flag) {
+ end_flag = 0;
+ if((gu16Delta[0] > gu16Threshold[0])||(gu16Delta[1] > gu16Threshold[1])) {
+ SliderPercentegePosition[0] = (gu16Delta[0]*100)/(gu16Delta[0]+gu16Delta[1]);
+ SliderPercentegePosition[1] = (gu16Delta[1]*100)/(gu16Delta[0]+gu16Delta[1]);
+ SliderDistancePosition[0] = (SliderPercentegePosition[0]* SLIDER_LENGTH)/100;
+ SliderDistancePosition[1] = (SliderPercentegePosition[1]* SLIDER_LENGTH)/100;
+ AbsolutePercentegePosition = ((100 - SliderPercentegePosition[0]) + SliderPercentegePosition[1])/2;
+ AbsoluteDistancePosition = ((SLIDER_LENGTH - SliderDistancePosition[0]) + SliderDistancePosition[1])/2;
+ } else {
+ SliderPercentegePosition[0] = NO_TOUCH;
+ SliderPercentegePosition[1] = NO_TOUCH;
+ SliderDistancePosition[0] = NO_TOUCH;
+ SliderDistancePosition[1] = NO_TOUCH;
+ AbsolutePercentegePosition = NO_TOUCH;
+ AbsoluteDistancePosition = NO_TOUCH;
+ }
+ }
+}
+
+float TSISensor::readPercentage() {
+ sliderRead();
+ return (float)AbsolutePercentegePosition/100.0;
+}
+
+uint8_t TSISensor::readDistance() {
+ sliderRead();
+ return AbsoluteDistancePosition;
+}
+
+static void changeElectrode(void)
+{
+ int16_t u16temp_delta;
+
+ gu16TSICount[ongoing_elec] = (TSI0->DATA & TSI_DATA_TSICNT_MASK); // Save Counts for current electrode
+ u16temp_delta = gu16TSICount[ongoing_elec] - gu16Baseline[ongoing_elec]; // Obtains Counts Delta from callibration reference
+ if(u16temp_delta < 0)
+ gu16Delta[ongoing_elec] = 0;
+ else
+ gu16Delta[ongoing_elec] = u16temp_delta;
+
+ //Change Electrode to Scan
+ if(total_electrode > 1)
+ {
+ if((total_electrode-1) > ongoing_elec)
+ ongoing_elec++;
+ else
+ ongoing_elec = 0;
+
+ TSI0->DATA = ((elec_array[ongoing_elec]<<TSI_DATA_TSICH_SHIFT) );
+ TSI0->DATA |= TSI_DATA_SWTS_MASK;
+ }
+}
+
+void tsi_irq(void)
+{
+ end_flag = 1;
+ TSI0->GENCS |= TSI_GENCS_EOSF_MASK; // Clear End of Scan Flag
+ changeElectrode();
+}
+
diff -r fe8665daf3e7 -r 089b778962c4 TSISensor.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/TSISensor.h Fri Oct 31 10:54:51 2014 +0000
@@ -0,0 +1,72 @@
+/* Freescale Semiconductor Inc.
+ * (c) Copyright 2004-2005 Freescale Semiconductor, Inc.
+ * (c) Copyright 2001-2004 Motorola, Inc.
+ *
+ * mbed Microcontroller Library
+ * (c) Copyright 2009-2012 ARM Limited.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
+ * and associated documentation files (the "Software"), to deal in the Software without
+ * restriction, including without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all copies or
+ * substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
+ * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
+ * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#ifndef TSISENSOR_H
+#define TSISENSOR_H
+
+/**
+* TSISensor example
+*
+* @code
+* #include "mbed.h"
+* #include "TSISensor.h"
+*
+* int main(void) {
+* PwmOut led(LED_GREEN);
+* TSISensor tsi;
+*
+* while (true) {
+* led = 1.0 - tsi.readPercentage();
+* wait(0.1);
+* }
+* }
+* @endcode
+*/
+class TSISensor {
+public:
+ /**
+ * Initialize the TSI Touch Sensor
+ */
+ TSISensor();
+
+ /**
+ * Read Touch Sensor percentage value
+ *
+ * @returns percentage value between [0 ... 1]
+ */
+ float readPercentage();
+
+ /**
+ * Read Touch Sensor distance
+ *
+ * @returns distance in mm. The value is between [0 ... 40]
+ */
+ uint8_t readDistance();
+
+private:
+ void sliderRead(void);
+ void selfCalibration(void);
+};
+
+#endif
+
diff -r fe8665daf3e7 -r 089b778962c4 camera_api.cpp
--- a/camera_api.cpp Mon Jul 07 06:52:03 2014 +0000
+++ b/camera_api.cpp Fri Oct 31 10:54:51 2014 +0000
@@ -276,7 +276,6 @@
line_CamR->enable();
- line_CamL->enable();
//input 128 //both
@@ -287,13 +286,44 @@
line_imageR[i]=line_CamR->read_u16();
- line_imageL[i]=line_CamL->read_u16();
+
// big small
if(line_imageR[i] > w_f_vR)
w_f_vR=line_imageR[i];
else if(line_imageR[i] < b_f_vR )
- b_f_vR = line_imageR[i];
+ b_f_vR = line_imageR[i];
+
+
+ *cam_clk=0;
+ wait_us(3);
+
+
+ }
+
+
+ line_CamR->disable();
+
+
+ *si=1;
+ *cam_clk=1;
+
+ wait_us(30); // tune here
+ *si=0;
+ *cam_clk=0;
+
+
+ line_CamL->enable();
+ //input 128 //both
+
+ for(int i=0;i<128;i++){
+ *cam_clk=1;
+ wait_us(3);
+
+
+ line_imageL[i]=line_CamL->read_u16();
+
+ // big small
if(line_imageL[i] > w_f_vL)
@@ -309,10 +339,7 @@
}
-
-
- line_CamR->enable();
- line_CamL->enable();
+ line_CamL->disable();
//filter L R //may change
diff -r fe8665daf3e7 -r 089b778962c4 main.cpp
--- a/main.cpp Mon Jul 07 06:52:03 2014 +0000
+++ b/main.cpp Fri Oct 31 10:54:51 2014 +0000
@@ -1,317 +1,275 @@
#include "mbed.h"
+#include "rtos.h"
#include "servo_api.h"
#include "camera_api.h"
#include "motor_api.h"
-
+#include "TSISensor.h"
+#include "pot.h"
#define Debug_cam_uart
-#define buffer_size 3
+#define buffer_size 6
+#define e_buffer_size 50
+//os
+Mutex stdio_mutex;
+BX_pot pot1('1');
+TSISensor tsiSensor;
+
Serial pc(USBTX, USBRX);
BX_servo servo;
BX_camera cam;
BX_motor motorA('A');
BX_motor motorB('B');
+int left_point_buffer[buffer_size] = {0,0,0,0,0,0};
+int right_point_buffer[buffer_size] = {0,0,0,0,0,0};
+int error_buffer[e_buffer_size] ={0};
+float angle_buffer[50]={0};
+int state = 0; //0: no line, 1:right line, 2:left line
+int last_state = 0;
+float turnAngle = 0.073;
+float Kp = 0.0;
-int main() {
-
- /*
- int black_va;
- int white_va;
- */
- int left_point_buffer[buffer_size] = {0,0,0};
- int right_point_buffer[buffer_size] = {0,0,0};
- int left_length_buffer[buffer_size] = {0,0,0};
- int right_length_buffer[buffer_size] = {0,0,0};
- int error_buffer[buffer_size]= {0,0,0};
- bool isLeftOut = false;
- bool isRightOut = false;
- servo.set_angle(0);
-#ifdef Debug_cam_uart
- pc.baud(115200);
-
- motorA.rotate(0.5);
- motorB.rotate(0.5);
-
- while(1){
-
+void servo_thread(void const *args){
+ while(1){
+ for(int i=0;i<49;i++){
+ angle_buffer[i] = angle_buffer[i+1];
+ }
+ angle_buffer[49] = turnAngle;
+
+ float average_angle=0;
+ for(int i=25;i<50;i++){
+ average_angle=average_angle+angle_buffer[i];
+ }
+ average_angle = average_angle/25;
+ float speed_error = average_angle - 0.073;
+ int Km_speed = 0;;
+ if(speed_error<0)
+ speed_error = speed_error*(-1);
+
+ if(speed_error <= 0.004 || speed_error >= 0.01){
+ Km_speed = -13;
+ }
+ else{
+ Km_speed = -7;
+ }
+
+ float speed = Km_speed*speed_error;
+ int int_max = pot1.read()*10;
+ float max = int_max/(10.0);
+ if(max+speed<0)
+ speed = 0.2-max;
+ motorA.rotate(max+speed);
+ motorB.rotate(max+speed);
+
+ //pc.printf("\r\n Turn Angle = %f \r\n",turnAngle );
+ servo.set_angle(turnAngle);
+ Thread :: wait(30);
+ }
+}
+
+float find_error(){
cam.read();
-
- //====== find left line's right edging ======
- bool left_isFistPoint = true;
- int left_temp_length = 0;
- int left_max_length = 0;
- int left_temp_point = 0;
- int left_target_point = 0;
- int left_white_count = 0;
- int left_white_threshold = 2;
- // scan camera data from right to left
+ int left_target_point = 0;
+ int left_average_point = 0;
+ int left_black_count = 0;
+ bool isLeftAllWhite = true;
for(int i=117;i>=10;i--){
- // when scan result is black and it is a start point
- if(cam.sign_line_imageL[i] == ' ' && left_isFistPoint){
- left_temp_point = i;
- left_isFistPoint = false;
- if(i == 10 && (left_max_length < left_temp_length)){
- left_max_length = left_temp_length;
- left_target_point = left_temp_point;
- }
- }
- // when scan result is black and it is not a start point
- else if(cam.sign_line_imageL[i] == ' ' && !left_isFistPoint){
- left_temp_length++;
- if(i == 10 && (left_max_length < left_temp_length)){
- left_max_length = left_temp_length;
- left_target_point = left_temp_point;
- }
- }
- // when scan result is white and there is no black behind it
- else if(cam.sign_line_imageL[i] != ' ' && left_isFistPoint){
- //do nothng
- }
- //when scan result is white and there are some black behind it
- else if(cam.sign_line_imageL[i] != ' ' && !left_isFistPoint){
- left_white_count++;
- //this white is still in the threshold
- if(left_white_count <= left_white_threshold){
- left_temp_length++;
- if(i == 10 && (left_max_length < left_temp_length)){
- left_max_length = left_temp_length;
- left_target_point = left_temp_point;
- }
- }
- //this white is out of threshold
- else{
- left_isFistPoint = true;
- if(left_max_length < left_temp_length){
- left_max_length = left_temp_length;
- left_target_point = left_temp_point;
- }
- left_temp_length = 0;
- left_white_count = 0;
- }
- }
+ if(cam.sign_line_imageL[i] == ' '){
+ left_black_count++;
+ if(left_black_count == 10){
+ left_target_point = i;
+ isLeftAllWhite = false;
+ break;
+ }
+ }
+ else{
+ left_black_count = 0;
+ }
}
- //average data
- int left_average_point = 0;
- int left_average_length = 0;
-
- //check line
- bool isLeftAllWhite = true;
- int left_black_count = 0;
- for(int i=10;i<=117;i++){
- if(cam.sign_line_imageL[i] == ' ')
- {
- left_black_count++;
- if( left_black_count >= 3 )
- {
- isLeftAllWhite = false;
- }
- }
- }
if(isLeftAllWhite){
//do not update the data
- //left_average_point = (left_point_buffer[0] + left_point_buffer[1] + left_point_buffer[2])/buffer_size;
- //left_average_length = (left_length_buffer[0] + left_length_buffer[1] + left_length_buffer[2])/buffer_size;
- left_average_point = left_point_buffer[buffer_size-1];
- left_average_length = left_length_buffer[buffer_size-1];
-
+ //left_average_point = (left_point_buffer[0] + left_point_buffer[1] + left_point_buffer[2] + left_point_buffer[3] +left_point_buffer[4]+left_point_buffer[5])/buffer_size;
+ left_average_point = left_point_buffer[5];
}
else{
- //average the history data
- //left_average_point = (left_point_buffer[0] + left_point_buffer[1] + left_point_buffer[2] + left_target_point)/(buffer_size+1);
- //left_average_length = (left_length_buffer[0] + left_length_buffer[1] + left_length_buffer[2] + left_max_length)/(buffer_size+1);
-
- left_average_point = left_target_point;
- left_average_length = left_max_length;
-
- //update the buffer
- for(int i=0;i<buffer_size - 1;i++){
- left_point_buffer[i] = left_point_buffer[i+1];
- left_length_buffer[i] = left_length_buffer[i+1];
- }
- left_point_buffer[buffer_size-1] = left_average_point;
- left_length_buffer[buffer_size-1] = left_average_length;
+ //average the history data
+ //left_average_point = (left_point_buffer[0] + left_point_buffer[1] + left_point_buffer[2] + left_point_buffer[3] +left_point_buffer[4]+left_point_buffer[5]+ left_target_point)/(buffer_size+1);
+ left_average_point = left_target_point;
}
//====== find right line's left edging ======
- bool right_isFistPoint = true;
- int right_temp_length = 0;
- int right_max_length = 0;
- int right_temp_point = 0;
- int right_target_point = 0;
- int right_white_count = 0;
- int right_white_threshold = 2;
- //scan camera data from right to left
- for(int i=10;i<=117;i++){
- //when scan result is black and it is a start point
- if(cam.sign_line_imageR[i] == ' ' && right_isFistPoint){
- right_temp_point = i;
- right_isFistPoint = false;
- if( i == 117 && (right_max_length < right_temp_length)){
- right_max_length = right_temp_length;
- right_target_point = right_temp_point;
- }
- }
- //when scan result is black and it is not a start point
- else if(cam.sign_line_imageR[i] == ' ' && !right_isFistPoint){
- right_temp_length++;
- if( i == 117 && (right_max_length < right_temp_length)){
- right_max_length = right_temp_length;
- right_target_point = right_temp_point;
- }
- }
- //when scan result is white and there is no black behind it
- else if(cam.sign_line_imageR[i] != ' ' && right_isFistPoint){
- //do nothng
- }
- //when scan result is white and there are some black behind it
- else if(cam.sign_line_imageR[i] != ' ' && !right_isFistPoint){
- right_white_count++;
- //this white is still in the threshold
- if(right_white_count <= right_white_threshold){
- right_temp_length++;
- if( i == 117 && (right_max_length < right_temp_length)){
- right_max_length = right_temp_length;
- right_target_point = right_temp_point;
- }
- }
- //this white is out of threshold
- else{
- right_isFistPoint = true;
- if(right_max_length < right_temp_length){
- right_max_length = right_temp_length;
- right_target_point = right_temp_point;
- }
- right_temp_length = 0;
- right_white_count = 0;
- }
- }
- }
-
- //average data
+ int right_target_point = 0;
int right_average_point = 0;
- int right_average_length = 0;
- //check line
+ int right_black_count = 0;
bool isRightAllWhite = true;
- int right_black_count = 0;
- for(int i=10;i<=117;i++){
- if(cam.sign_line_imageR[i] == ' ')
- {
+ for(int i=10;i<118;i++){
+ if(cam.sign_line_imageR[i] == ' '){
right_black_count++;
- if( right_black_count >= 3 )
- {
+ if(right_black_count == 10){
+ right_target_point = i;
isRightAllWhite = false;
- }
+ break;
+ }
+ }
+ else{
+ right_black_count = 0;
}
}
+
+ /*for(int i=118;i>=10;i--){
+ if(i<left_average_point){
+ pc.printf(" ");
+ }
+ else
+ pc.printf("O");
+ }*/
+
+
+
if(isRightAllWhite){
//do not update the buffer
- //right_average_point = (right_point_buffer[0] + right_point_buffer[1] + right_point_buffer[2])/buffer_size;
- //right_average_length = (right_length_buffer[0] + right_length_buffer[1] + right_length_buffer[2])/buffer_size;
- right_average_point = right_point_buffer[buffer_size-1];
- right_average_length = right_length_buffer[buffer_size-1];
+ //right_average_point = (right_point_buffer[0] + right_point_buffer[1] + right_point_buffer[2] + right_point_buffer[3] + right_point_buffer[4] + right_point_buffer[5])/buffer_size;
+ right_average_point = right_point_buffer[5];
}
else{
//average the history data
- //right_average_point = (right_point_buffer[0] + right_point_buffer[1] + right_point_buffer[2] + right_target_point)/(buffer_size+1);
- //right_average_length = (right_length_buffer[0] + right_length_buffer[1] + right_length_buffer[2] + right_max_length)/(buffer_size+1);
+ //right_average_point = (right_point_buffer[0] + right_point_buffer[1] + right_point_buffer[2] + right_point_buffer[3] + right_point_buffer[4] + right_point_buffer[5] + right_target_point)/(buffer_size+1);
right_average_point = right_target_point;
- right_average_length = right_max_length;
-
- //update the buffer
- for(int i=0;i<buffer_size-1;i++){
- right_point_buffer[i] = right_point_buffer[i+1];
- right_length_buffer[i] = right_length_buffer[i+1];
+ }
+ /*
+ for(int i=10;i<118;i++){
+ if(i>=right_average_point){
+ pc.printf(" ");
}
- right_point_buffer[buffer_size-1] = right_average_point;
- right_length_buffer[buffer_size-1] = right_average_length;
- }
-
- //print left camera result
-
- for(int i=0;i<=127;i++){
- if( i >= left_average_point-left_average_length && i<=left_average_point)
- pc.printf(" ");
else
- pc.printf("x");
+ pc.printf("O");
+ }
+ pc.printf("\r\n");*/
+ //update buffer
+ for(int i=0;i<buffer_size-1;i++){
+ right_point_buffer[i] = right_point_buffer[i+1];
+ left_point_buffer[i] = left_point_buffer[i+1];
}
- //print right camera result
-
- for(int i=0;i<=127;i++){
- if( i <= right_average_point+right_average_length && i>=right_average_point)
- pc.printf(" ");
- else
- pc.printf("x");
- }
-
-
- pc.printf("\n L_center : %d , R_center : %d \r\n", left_average_point,right_average_point);
- pc.printf("\r\n");
+ left_point_buffer[buffer_size-1] = left_average_point;
+ right_point_buffer[buffer_size-1] = right_average_point;
- //PID -- P
- int error = 0;
- int turn = 0;
- int center = 55;
- float Kp = 0;
- float turn_angle = 0;
- int reference_point= 0;
-
+ int center = 0;
+ int reference_point = 0;
if(isLeftAllWhite && !isRightAllWhite){
- error = 40;
- turn = 90;
+ state = 1;
+ Kp = 0.0004;
+ center = 102;
reference_point = right_average_point;
+
}
else if(!isLeftAllWhite && isRightAllWhite){
- error = 40;
- turn = 90;
+ state = 2;
+ Kp = 0.0004;
+ center = 20;
reference_point = left_average_point;
}
else if(isLeftAllWhite && isRightAllWhite){
- error = 30;
- turn = 90;
- if(left_average_point < 40 && right_average_point < 40)
- reference_point = 0;
- else if(left_average_point > 70 && right_average_point > 70)
- reference_point = 90;
-
- //pc.printf("\n allllllllllllllllllllll whiteeeeeeeeeeeeeee \n");
+ state = 0;
+ Kp = 0.0004;
+ center = 64;
+ //straight line
+ float average_angle;
+ for(int i=0;i<50;i++){
+ average_angle = average_angle + angle_buffer[i];
+ }
+ average_angle = average_angle/50;
+ if(average_angle > 0.064 && average_angle < 0.08){
+ Kp = 0;
+ }
+ else{
+ if(left_average_point <= 40 && right_average_point <= 40)
+ reference_point = 15;
+ else if(left_average_point >= 70 && right_average_point >= 70)
+ reference_point = 95;
+ }
}
else if(!isLeftAllWhite && !isRightAllWhite){
- error = 50;
- turn = 90;
- center = 108;
- reference_point = (right_average_point + 108 + left_average_point)/2;
+ Kp = 0.0002;
+
+ if(128 - left_average_point >= right_average_point)
+ {
+ state = 1;
+ center = 95;
+ reference_point = right_average_point;
+ }
+ else{
+ state = 2;
+ center = 15;
+ reference_point = left_average_point;
+ }
+ }
+
+
+ return reference_point - center;
+}
+
+
+int main() {
+
+ while (1)
+ {
+ if (tsiSensor.readPercentage() > 0.00011)
+ {
+ Thread :: wait(2000);
+ break;
+ }
}
-
- Kp = turn/error;
+
+ servo.set_angle(0.073);
+
+#ifdef Debug_cam_uart
+ pc.baud(115200);
+
- //PID -- I
- /*for(int i=0;i<4;i++){
- error_buffer[i] = error_buffer[i+1];
- }
- error_buffer[4] = reference_point - center;
- float integral = 0;
- float Ki = 1.1f;
- for(int i=0;i<5;i++){
- integral = (2/3)*integral + error_buffer[i];
- }
- turn_angle = (int) (Kp*(reference_point - center) + Ki*integral);*/
+
+
+ motorA.rotate(0.33);
+ motorB.rotate(0.33);
+ Thread th_s(servo_thread);
+ while(1){
+ if(last_state != state){
+ for(int i=0;i<e_buffer_size - 1;i++){
+ error_buffer[i] = 0;
+ }
+ }
+ else{
+ for(int i=0;i<e_buffer_size - 1;i++){
+ error_buffer[i] = error_buffer[i+1];
+ }
+ }
+ error_buffer[e_buffer_size-1] = find_error();
- turn_angle = (int) (Kp*(reference_point - center));
- if(turn_angle >= 50)
- turn_angle = 50;
- if(turn_angle <= -50)
- turn_angle = -50;
+ float past_error = 0;
+ for(int i= 0;i<e_buffer_size;i++){
+ past_error = (past_error*2)/3 + error_buffer[i];
+ }
+ past_error = past_error/e_buffer_size;
+ float future_error = (error_buffer[e_buffer_size-1] - error_buffer[0])/e_buffer_size + error_buffer[e_buffer_size-1];
- servo.set_angle(turn_angle);
- }
-
-
+ stdio_mutex.lock();
+ turnAngle = 0.073 + Kp*(error_buffer[e_buffer_size-1]
+ + (1*past_error)/9
+ + (4*future_error)/9);
+ stdio_mutex.unlock();
+ last_state = state;
+
+
+ // pc.printf("%f \r\n",pot1.read());
+
+
+ Thread :: wait(5);
+ }
+
#endif
diff -r fe8665daf3e7 -r 089b778962c4 mbed-rtos.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-rtos.lib Fri Oct 31 10:54:51 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed-rtos/#512640f00564
diff -r fe8665daf3e7 -r 089b778962c4 motor_api.cpp
--- a/motor_api.cpp Mon Jul 07 06:52:03 2014 +0000
+++ b/motor_api.cpp Fri Oct 31 10:54:51 2014 +0000
@@ -60,11 +60,11 @@
case 'A':
*forward_A=level;
- *backward_A=0;
+ *backward_A=0.1;
break;
case 'B':
*forward_B=level;
- *backward_B=0;
+ *backward_B=0.1;
break;
}
@@ -75,11 +75,11 @@
switch(Type){
case 'A':
- *forward_A=0;
+ *forward_A=0.1;
*backward_A=level;
break;
case 'B':
- *forward_B=0;
+ *forward_B=0.1;
*backward_B=level;
break;
}
diff -r fe8665daf3e7 -r 089b778962c4 pot.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/pot.cpp Fri Oct 31 10:54:51 2014 +0000
@@ -0,0 +1,30 @@
+
+#include "mbed.h"
+#include "pot.h"
+
+
+
+BX_pot::BX_pot(char t){
+
+
+ switch (t){
+ case '1':
+ pot_in = new AnalogIn (PTB3);
+ break;
+ case '2':
+ pot_in = new AnalogIn (PTB2);
+ break;
+ }
+
+
+
+ }
+
+ float BX_pot::read(void){
+
+
+
+ return pot_in->read();
+
+
+}
\ No newline at end of file
diff -r fe8665daf3e7 -r 089b778962c4 pot.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/pot.h Fri Oct 31 10:54:51 2014 +0000
@@ -0,0 +1,33 @@
+
+
+
+
+#include "mbed.h"
+class BX_pot{
+
+ public:
+
+ //static
+ float read(void); //block in here
+
+ BX_pot(char t);
+
+
+
+
+
+
+
+
+ private:
+
+ AnalogIn* pot_in;
+
+
+
+
+
+
+
+
+ };
\ No newline at end of file
diff -r fe8665daf3e7 -r 089b778962c4 servo_api.cpp
--- a/servo_api.cpp Mon Jul 07 06:52:03 2014 +0000
+++ b/servo_api.cpp Fri Oct 31 10:54:51 2014 +0000
@@ -1,47 +1,45 @@
// 0~180 angle 1~2ms
#include "mbed.h"
#include "servo_api.h"
-
-
-#define right_end 0.05 //90
-
-#define left_end 0.1 //-90
-
+
+
+#define right_end 0.088 // 0.088
+ //MID 0.073
+#define left_end 0.058 // 0.058
+
//memory opt
// 5 degree seperate
-
-
-
+
+
+
BX_servo::BX_servo(void){
angle = 0;
-
+
servo_in= new PwmOut(PTB0);
servo_in->period_ms(20);
- for(int i=0;i<37;i++){
-
- angle_level[i]=i*(0.05/36)+right_end;
- }
-
- *servo_in =angle_level[18];
-
+
}
-
-
-
-
-
-
-int BX_servo::set_angle(int a){
-
-
- angle=a;
+
+
+
+
+
+
+float BX_servo::set_angle(float a){
+
+ if( a<left_end )
+ a=left_end;
+ else if(a> right_end)
+ a=right_end;
- *servo_in=angle_level[18+a/5];
+ angle=a;
+
+ *servo_in=angle;
diff -r fe8665daf3e7 -r 089b778962c4 servo_api.h
--- a/servo_api.h Mon Jul 07 06:52:03 2014 +0000
+++ b/servo_api.h Fri Oct 31 10:54:51 2014 +0000
@@ -1,15 +1,17 @@
+
+
#include "mbed.h"
-
-
-
+
+
+
class BX_servo{
public:
- int set_angle(int a);
+ float set_angle(float a);
BX_servo(void);
@@ -22,10 +24,11 @@
private:
//-90~0~90
- int angle ;
+ float angle ;
PwmOut* servo_in;
//0~36 // 18 mid
- float angle_level[37];
+
- };
\ No newline at end of file
+ };
+