fatboyslim / Mbed 2 deprecated buttontest

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:   FRDM-TFC mbed

main.cpp@12:288df1e95dec, 2015-02-23 (annotated)

Committer:
bbbobbbieo
Date:
Mon Feb 23 21:12:31 2015 +0000
Revision:
12:288df1e95dec
Parent:
11:d65d6d7fc85e
Child:
13:6ad980fd3394
turns the servo based on the center line of track;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bbbobbbieo 3:c7caa058fc50 1 //#include "mbed.h"
bbbobbbieo 1:21d40d90b2f0 2 #include "TFC.h"
bbbobbbieo 0:d57117b2188d 3
bbbobbbieo 0:d57117b2188d 4 DigitalOut myled(LED1);
bbbobbbieo 0:d57117b2188d 5
bbbobbbieo 1:21d40d90b2f0 6 int main()
bbbobbbieo 1:21d40d90b2f0 7 {
bbbobbbieo 7:455e7dd338ee 8 //run this before anything
bbbobbbieo 2:d8a51492b646 9 TFC_Init();
bbbobbbieo 2:d8a51492b646 10
bbbobbbieo 7:455e7dd338ee 11 //variables
bbbobbbieo 4:7584ff0426f1 12 float current_servo_position = 0;
bbbobbbieo 6:44d1079f076c 13 float current_left_motor_speed = 0;
bbbobbbieo 6:44d1079f076c 14 float current_right_motor_speed = 0;
bbbobbbieo 6:44d1079f076c 15 bool rear_motor_enable_flag = true;
bbbobbbieo 7:455e7dd338ee 16 bool linescan_ping_pong = false;
bbbobbbieo 9:2b028ee421ad 17 bool linescan_enable = true;
bbbobbbieo 4:7584ff0426f1 18
bbbobbbieo 11:d65d6d7fc85e 19 int black_values_list[128];
bbbobbbieo 11:d65d6d7fc85e 20 int black_value_count = 0;
bbbobbbieo 11:d65d6d7fc85e 21 int black_center_value = 0;
bbbobbbieo 11:d65d6d7fc85e 22 int sum_black = 0;
bbbobbbieo 11:d65d6d7fc85e 23
bbbobbbieo 11:d65d6d7fc85e 24
bbbobbbieo 9:2b028ee421ad 25 //uint16_t MyImage0Buffer[2][128];
bbbobbbieo 9:2b028ee421ad 26 //uint16_t MyImage1Buffer[2][128];
bbbobbbieo 7:455e7dd338ee 27
bbbobbbieo 7:455e7dd338ee 28
bbbobbbieo 7:455e7dd338ee 29 // major loop
bbbobbbieo 1:21d40d90b2f0 30 while(1)
bbbobbbieo 1:21d40d90b2f0 31 {
bbbobbbieo 2:d8a51492b646 32
bbbobbbieo 3:c7caa058fc50 33 if (TFC_ReadPushButton(0) != 0 )
bbbobbbieo 3:c7caa058fc50 34 {
bbbobbbieo 9:2b028ee421ad 35 /*(TFC_BAT_LED0_ON;
bbbobbbieo 6:44d1079f076c 36 wait(0.004);
bbbobbbieo 3:c7caa058fc50 37 TFC_BAT_LED0_OFF;
bbbobbbieo 9:2b028ee421ad 38 wait(0.004);*/
bbbobbbieo 4:7584ff0426f1 39
bbbobbbieo 6:44d1079f076c 40 current_servo_position = current_servo_position-.005;
bbbobbbieo 6:44d1079f076c 41 if(current_servo_position <= -0.4)
bbbobbbieo 6:44d1079f076c 42 current_servo_position = -0.4;
bbbobbbieo 5:e7e4f1967fb8 43 TFC_SetServo(0, current_servo_position);
bbbobbbieo 7:455e7dd338ee 44 }// end check button0
bbbobbbieo 4:7584ff0426f1 45
bbbobbbieo 3:c7caa058fc50 46 else
bbbobbbieo 3:c7caa058fc50 47 {
bbbobbbieo 9:2b028ee421ad 48 //TFC_BAT_LED0_ON;
bbbobbbieo 3:c7caa058fc50 49 }
bbbobbbieo 4:7584ff0426f1 50
bbbobbbieo 4:7584ff0426f1 51 if (TFC_ReadPushButton(1) != 0 )
bbbobbbieo 4:7584ff0426f1 52 {
bbbobbbieo 9:2b028ee421ad 53 /*TFC_BAT_LED1_ON;
bbbobbbieo 6:44d1079f076c 54 wait(0.004);
bbbobbbieo 4:7584ff0426f1 55 TFC_BAT_LED1_OFF;
bbbobbbieo 9:2b028ee421ad 56 wait(0.004);*/
bbbobbbieo 4:7584ff0426f1 57
bbbobbbieo 6:44d1079f076c 58 current_servo_position = current_servo_position+.005;
bbbobbbieo 6:44d1079f076c 59 if(current_servo_position >= 0.4)
bbbobbbieo 6:44d1079f076c 60 current_servo_position = 0.4;
bbbobbbieo 5:e7e4f1967fb8 61 TFC_SetServo(0, current_servo_position);
bbbobbbieo 7:455e7dd338ee 62 }// end check button1
bbbobbbieo 4:7584ff0426f1 63
bbbobbbieo 4:7584ff0426f1 64 else
bbbobbbieo 4:7584ff0426f1 65 {
bbbobbbieo 9:2b028ee421ad 66 //TFC_BAT_LED1_ON;
bbbobbbieo 4:7584ff0426f1 67 }
bbbobbbieo 4:7584ff0426f1 68
bbbobbbieo 6:44d1079f076c 69 if(rear_motor_enable_flag)
bbbobbbieo 6:44d1079f076c 70 {
bbbobbbieo 7:455e7dd338ee 71 TFC_HBRIDGE_ENABLE;
bbbobbbieo 6:44d1079f076c 72 //current_left_motor_speed = current_left_motor_speed + .3*(TFC_ReadPot(0));
bbbobbbieo 6:44d1079f076c 73 //current_right_motor_speed = current_right_motor_speed + .3*(TFC_ReadPot(1));
bbbobbbieo 6:44d1079f076c 74
bbbobbbieo 7:455e7dd338ee 75 //current_left_motor_speed = .3*(TFC_ReadPot(0));
bbbobbbieo 7:455e7dd338ee 76 //current_right_motor_speed = .3*(TFC_ReadPot(1));
bbbobbbieo 7:455e7dd338ee 77
bbbobbbieo 7:455e7dd338ee 78 current_left_motor_speed = (TFC_ReadPot(0));
bbbobbbieo 7:455e7dd338ee 79 current_right_motor_speed = (TFC_ReadPot(1));
bbbobbbieo 6:44d1079f076c 80
bbbobbbieo 6:44d1079f076c 81 if(current_left_motor_speed >= 0.4)
bbbobbbieo 6:44d1079f076c 82 current_left_motor_speed= 0.4;
bbbobbbieo 6:44d1079f076c 83 if(current_right_motor_speed >= 0.4)
bbbobbbieo 6:44d1079f076c 84 current_right_motor_speed= 0.4;
bbbobbbieo 6:44d1079f076c 85 if(current_left_motor_speed <= -0.4)
bbbobbbieo 6:44d1079f076c 86 current_left_motor_speed= -0.4;
bbbobbbieo 6:44d1079f076c 87 if(current_right_motor_speed <= -0.4)
bbbobbbieo 6:44d1079f076c 88 current_right_motor_speed= -0.4;
bbbobbbieo 6:44d1079f076c 89
bbbobbbieo 6:44d1079f076c 90 TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
bbbobbbieo 7:455e7dd338ee 91 }// end motor enabled
bbbobbbieo 7:455e7dd338ee 92 else
bbbobbbieo 7:455e7dd338ee 93 {
bbbobbbieo 7:455e7dd338ee 94 TFC_HBRIDGE_DISABLE;
bbbobbbieo 7:455e7dd338ee 95 }// end motor disabled
bbbobbbieo 7:455e7dd338ee 96
bbbobbbieo 7:455e7dd338ee 97 if (linescan_enable)
bbbobbbieo 7:455e7dd338ee 98 {
bbbobbbieo 7:455e7dd338ee 99 if (TFC_LineScanImageReady !=0)
bbbobbbieo 7:455e7dd338ee 100 {
bbbobbbieo 11:d65d6d7fc85e 101
bbbobbbieo 7:455e7dd338ee 102 if (linescan_ping_pong) //checking channel 0
bbbobbbieo 7:455e7dd338ee 103 {
bbbobbbieo 7:455e7dd338ee 104 //...
bbbobbbieo 9:2b028ee421ad 105 //uint8_t shitnum = uint8_t(*TFC_LineScanImage0>>3);
bbbobbbieo 9:2b028ee421ad 106 uint8_t shitnum = 1;
bbbobbbieo 10:6d9d70a240fd 107 if (*(TFC_LineScanImage0+64) > 800)
bbbobbbieo 9:2b028ee421ad 108 shitnum = 15;
bbbobbbieo 10:6d9d70a240fd 109 else if((*(TFC_LineScanImage0+64) > 450))
bbbobbbieo 9:2b028ee421ad 110 shitnum = 7;
bbbobbbieo 10:6d9d70a240fd 111 else if((*(TFC_LineScanImage0+64) > 400))
bbbobbbieo 9:2b028ee421ad 112 shitnum = 3;
bbbobbbieo 9:2b028ee421ad 113 else
bbbobbbieo 9:2b028ee421ad 114 shitnum = 1;
bbbobbbieo 9:2b028ee421ad 115
bbbobbbieo 9:2b028ee421ad 116 TFC_SetBatteryLED(shitnum);
bbbobbbieo 11:d65d6d7fc85e 117
bbbobbbieo 11:d65d6d7fc85e 118 // checking for center line?
bbbobbbieo 11:d65d6d7fc85e 119 for (uint16_t i=0; i<128; i++)
bbbobbbieo 11:d65d6d7fc85e 120 {
bbbobbbieo 11:d65d6d7fc85e 121 if ((*(TFC_LineScanImage0+i) > 450))
bbbobbbieo 11:d65d6d7fc85e 122 {
bbbobbbieo 11:d65d6d7fc85e 123 black_values_list[black_value_count] = i;
bbbobbbieo 11:d65d6d7fc85e 124 black_value_count++;
bbbobbbieo 11:d65d6d7fc85e 125 }
bbbobbbieo 11:d65d6d7fc85e 126 }
bbbobbbieo 11:d65d6d7fc85e 127
bbbobbbieo 11:d65d6d7fc85e 128 for(int i=0; i<black_value_count; i++)
bbbobbbieo 11:d65d6d7fc85e 129 {
bbbobbbieo 11:d65d6d7fc85e 130 sum_black += black_values_list[i];
bbbobbbieo 11:d65d6d7fc85e 131 }
bbbobbbieo 11:d65d6d7fc85e 132
bbbobbbieo 11:d65d6d7fc85e 133 black_center_value= sum_black / black_value_count;
bbbobbbieo 11:d65d6d7fc85e 134
bbbobbbieo 11:d65d6d7fc85e 135 if (black_center_value > 64)
bbbobbbieo 11:d65d6d7fc85e 136 {
bbbobbbieo 12:288df1e95dec 137 current_servo_position = current_servo_position-.0001;
bbbobbbieo 11:d65d6d7fc85e 138 if(current_servo_position <= -0.4)
bbbobbbieo 11:d65d6d7fc85e 139 current_servo_position = -0.4;
bbbobbbieo 11:d65d6d7fc85e 140 TFC_SetServo(0, current_servo_position);
bbbobbbieo 11:d65d6d7fc85e 141 }
bbbobbbieo 11:d65d6d7fc85e 142
bbbobbbieo 11:d65d6d7fc85e 143 if (black_center_value < 64)
bbbobbbieo 11:d65d6d7fc85e 144 {
bbbobbbieo 12:288df1e95dec 145 current_servo_position = current_servo_position+.0001;
bbbobbbieo 12:288df1e95dec 146 if( current_servo_position >= +0.4)
bbbobbbieo 11:d65d6d7fc85e 147 current_servo_position = +0.4;
bbbobbbieo 11:d65d6d7fc85e 148 TFC_SetServo(0, current_servo_position);
bbbobbbieo 11:d65d6d7fc85e 149 }
bbbobbbieo 11:d65d6d7fc85e 150
bbbobbbieo 11:d65d6d7fc85e 151 black_value_count = 0;
bbbobbbieo 11:d65d6d7fc85e 152 black_center_value = 0;
bbbobbbieo 11:d65d6d7fc85e 153 sum_black = 0;
bbbobbbieo 11:d65d6d7fc85e 154
bbbobbbieo 11:d65d6d7fc85e 155 // end checking for center line
bbbobbbieo 11:d65d6d7fc85e 156
bbbobbbieo 11:d65d6d7fc85e 157
bbbobbbieo 7:455e7dd338ee 158 linescan_ping_pong = false;
bbbobbbieo 11:d65d6d7fc85e 159 } // end checking channel 0
bbbobbbieo 7:455e7dd338ee 160 else //checking channel 1
bbbobbbieo 7:455e7dd338ee 161 {
bbbobbbieo 7:455e7dd338ee 162 //...
bbbobbbieo 9:2b028ee421ad 163 //TFC_SetBatteryLED(*TFC_LineScanImage1+4);
bbbobbbieo 7:455e7dd338ee 164 linescan_ping_pong = true;
bbbobbbieo 7:455e7dd338ee 165 }
bbbobbbieo 11:d65d6d7fc85e 166
bbbobbbieo 7:455e7dd338ee 167
bbbobbbieo 7:455e7dd338ee 168 TFC_LineScanImageReady ==0; // since we used it, we reset the flag
bbbobbbieo 7:455e7dd338ee 169 }// end imageready
bbbobbbieo 7:455e7dd338ee 170 }// end linescan stuff
bbbobbbieo 0:d57117b2188d 171 }
bbbobbbieo 0:d57117b2188d 172 }
bbbobbbieo 8:946806df7347 173
bbbobbbieo 8:946806df7347 174
bbbobbbieo 8:946806df7347 175
bbbobbbieo 8:946806df7347 176 // shit code
bbbobbbieo 8:946806df7347 177
bbbobbbieo 8:946806df7347 178 //this block is the worst image processing algorithm ever
bbbobbbieo 8:946806df7347 179 /*
bbbobbbieo 8:946806df7347 180 black_values_list[];
bbbobbbieo 8:946806df7347 181 black_value_count = 0;
bbbobbbieo 8:946806df7347 182 black_center_value = 0;
bbbobbbieo 8:946806df7347 183 sum_black = 0;
bbbobbbieo 8:946806df7347 184
bbbobbbieo 8:946806df7347 185 for (int i=0; i<128; i++)
bbbobbbieo 8:946806df7347 186 {
bbbobbbieo 8:946806df7347 187 if (camera_data[i]< 100)
bbbobbbieo 8:946806df7347 188 {
bbbobbbieo 8:946806df7347 189 black_values_list[black_value_count] = i;
bbbobbbieo 8:946806df7347 190 black_value_count++;
bbbobbbieo 8:946806df7347 191 }
bbbobbbieo 8:946806df7347 192 }
bbbobbbieo 8:946806df7347 193
bbbobbbieo 8:946806df7347 194 for(int i=0; i<black_value_count; i++)
bbbobbbieo 8:946806df7347 195 {
bbbobbbieo 8:946806df7347 196 sum_black += black_values_list[i];
bbbobbbieo 8:946806df7347 197 }
bbbobbbieo 8:946806df7347 198
bbbobbbieo 8:946806df7347 199 black_center_value= sum_black / black_value_count;
bbbobbbieo 8:946806df7347 200
bbbobbbieo 8:946806df7347 201 if (black_center_value > 64)
bbbobbbieo 8:946806df7347 202 servo_left();
bbbobbbieo 8:946806df7347 203
bbbobbbieo 8:946806df7347 204 if (black_center_value < 64)
bbbobbbieo 8:946806df7347 205 servo_right();
bbbobbbieo 8:946806df7347 206 */
bbbobbbieo 8:946806df7347 207
bbbobbbieo 8:946806df7347 208