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@18:3988310cd03b, 2015-02-26 (annotated)

Committer:
bbbobbbieo
Date:
Thu Feb 26 16:17:12 2015 +0000
Revision:
18:3988310cd03b
Parent:
16:11ba5d6f42ba 
less reverse in a turn

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 14:f43b386b8b5d 6 int main()
bbbobbbieo 1:21d40d90b2f0 7 {
bbbobbbieo 7:455e7dd338ee 8 //run this before anything
bbbobbbieo 2:d8a51492b646 9 TFC_Init();
bbbobbbieo 14:f43b386b8b5d 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 14:f43b386b8b5d 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 14:f43b386b8b5d 23 int violence_level = 0;
bbbobbbieo 15:830209e846d5 24
bbbobbbieo 9:2b028ee421ad 25 //uint16_t MyImage0Buffer[2][128];
bbbobbbieo 9:2b028ee421ad 26 //uint16_t MyImage1Buffer[2][128];
bbbobbbieo 14:f43b386b8b5d 27
bbbobbbieo 7:455e7dd338ee 28 // major loop
bbbobbbieo 14:f43b386b8b5d 29 while(1) {
bbbobbbieo 14:f43b386b8b5d 30
bbbobbbieo 15:830209e846d5 31 // manual servo control, unused
bbbobbbieo 14:f43b386b8b5d 32 if (TFC_ReadPushButton(0) != 0 ) {
bbbobbbieo 14:f43b386b8b5d 33 current_servo_position = current_servo_position-.005;
bbbobbbieo 14:f43b386b8b5d 34 if(current_servo_position <= -0.4)
bbbobbbieo 14:f43b386b8b5d 35 current_servo_position = -0.4;
bbbobbbieo 14:f43b386b8b5d 36 TFC_SetServo(0, current_servo_position);
bbbobbbieo 14:f43b386b8b5d 37 }// end check button0
bbbobbbieo 14:f43b386b8b5d 38
bbbobbbieo 15:830209e846d5 39 else {}
bbbobbbieo 14:f43b386b8b5d 40
bbbobbbieo 15:830209e846d5 41 // manual servo control, unused
bbbobbbieo 14:f43b386b8b5d 42 if (TFC_ReadPushButton(1) != 0 ) {
bbbobbbieo 14:f43b386b8b5d 43 current_servo_position = current_servo_position+.005;
bbbobbbieo 14:f43b386b8b5d 44 if(current_servo_position >= 0.4)
bbbobbbieo 14:f43b386b8b5d 45 current_servo_position = 0.4;
bbbobbbieo 14:f43b386b8b5d 46 TFC_SetServo(0, current_servo_position);
bbbobbbieo 14:f43b386b8b5d 47 }// end check button1
bbbobbbieo 14:f43b386b8b5d 48
bbbobbbieo 15:830209e846d5 49 else {}
bbbobbbieo 14:f43b386b8b5d 50
bbbobbbieo 15:830209e846d5 51 // initial motor stuff
bbbobbbieo 14:f43b386b8b5d 52 if(rear_motor_enable_flag) {
bbbobbbieo 14:f43b386b8b5d 53 TFC_HBRIDGE_ENABLE;
bbbobbbieo 14:f43b386b8b5d 54
bbbobbbieo 15:830209e846d5 55 // checking behavior level
bbbobbbieo 14:f43b386b8b5d 56 violence_level = int(TFC_GetDIP_Switch());
bbbobbbieo 14:f43b386b8b5d 57
bbbobbbieo 15:830209e846d5 58 if (violence_level==2) {
bbbobbbieo 16:11ba5d6f42ba 59 current_left_motor_speed = -.48;
bbbobbbieo 16:11ba5d6f42ba 60 current_right_motor_speed = .48;
bbbobbbieo 15:830209e846d5 61 }
bbbobbbieo 15:830209e846d5 62 else if (violence_level==1) {
bbbobbbieo 16:11ba5d6f42ba 63 current_left_motor_speed = -.35;
bbbobbbieo 16:11ba5d6f42ba 64 current_right_motor_speed = .35;
bbbobbbieo 14:f43b386b8b5d 65 }
bbbobbbieo 15:830209e846d5 66 else if (violence_level==0) {
bbbobbbieo 15:830209e846d5 67 current_left_motor_speed = 0;
bbbobbbieo 15:830209e846d5 68 current_right_motor_speed = 0;
bbbobbbieo 15:830209e846d5 69 }
bbbobbbieo 15:830209e846d5 70 else {
bbbobbbieo 15:830209e846d5 71 current_left_motor_speed = 0;
bbbobbbieo 15:830209e846d5 72 current_right_motor_speed = 0;
bbbobbbieo 15:830209e846d5 73 }
bbbobbbieo 15:830209e846d5 74
bbbobbbieo 15:830209e846d5 75 // protection block
bbbobbbieo 15:830209e846d5 76 if(current_left_motor_speed >= 0.5)
bbbobbbieo 15:830209e846d5 77 current_left_motor_speed= 0.5;
bbbobbbieo 15:830209e846d5 78 if(current_right_motor_speed >= 0.5)
bbbobbbieo 15:830209e846d5 79 current_right_motor_speed= 0.5;
bbbobbbieo 15:830209e846d5 80 if(current_left_motor_speed <= -0.5)
bbbobbbieo 15:830209e846d5 81 current_left_motor_speed= -0.5;
bbbobbbieo 15:830209e846d5 82 if(current_right_motor_speed <= -0.5)
bbbobbbieo 15:830209e846d5 83 current_right_motor_speed= -0.5;
bbbobbbieo 14:f43b386b8b5d 84
bbbobbbieo 14:f43b386b8b5d 85 TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
bbbobbbieo 14:f43b386b8b5d 86 }// end motor enabled
bbbobbbieo 14:f43b386b8b5d 87 else {
bbbobbbieo 14:f43b386b8b5d 88 TFC_HBRIDGE_DISABLE;
bbbobbbieo 14:f43b386b8b5d 89 }// end motor disabled
bbbobbbieo 14:f43b386b8b5d 90
bbbobbbieo 15:830209e846d5 91 // camera stuff
bbbobbbieo 14:f43b386b8b5d 92 if (linescan_enable) {
bbbobbbieo 14:f43b386b8b5d 93 if (TFC_LineScanImageReady !=0) {
bbbobbbieo 14:f43b386b8b5d 94
bbbobbbieo 15:830209e846d5 95 if (linescan_ping_pong) {
bbbobbbieo 15:830209e846d5 96 //checking channel 0
bbbobbbieo 15:830209e846d5 97
bbbobbbieo 15:830209e846d5 98 //checking center pixel, displays aprox value on leds
bbbobbbieo 14:f43b386b8b5d 99 uint8_t shitnum = 1;
bbbobbbieo 14:f43b386b8b5d 100 if (*(TFC_LineScanImage0+64) > 800)
bbbobbbieo 14:f43b386b8b5d 101 shitnum = 15;
bbbobbbieo 14:f43b386b8b5d 102 else if((*(TFC_LineScanImage0+64) > 450))
bbbobbbieo 14:f43b386b8b5d 103 shitnum = 7;
bbbobbbieo 14:f43b386b8b5d 104 else if((*(TFC_LineScanImage0+64) > 400))
bbbobbbieo 14:f43b386b8b5d 105 shitnum = 3;
bbbobbbieo 14:f43b386b8b5d 106 else
bbbobbbieo 14:f43b386b8b5d 107 shitnum = 1;
bbbobbbieo 14:f43b386b8b5d 108 TFC_SetBatteryLED(shitnum);
bbbobbbieo 14:f43b386b8b5d 109
bbbobbbieo 15:830209e846d5 110
bbbobbbieo 15:830209e846d5 111 // checking for center line (single line)
bbbobbbieo 14:f43b386b8b5d 112 for (uint16_t i=0; i<128; i++) {
bbbobbbieo 14:f43b386b8b5d 113 if ((*(TFC_LineScanImage0+i) < 300)) {
bbbobbbieo 14:f43b386b8b5d 114 black_values_list[black_value_count] = i;
bbbobbbieo 14:f43b386b8b5d 115 black_value_count++;
bbbobbbieo 14:f43b386b8b5d 116 }
bbbobbbieo 14:f43b386b8b5d 117 }
bbbobbbieo 14:f43b386b8b5d 118
bbbobbbieo 14:f43b386b8b5d 119 for(int i=0; i<black_value_count; i++) {
bbbobbbieo 14:f43b386b8b5d 120 sum_black += black_values_list[i];
bbbobbbieo 14:f43b386b8b5d 121 }
bbbobbbieo 14:f43b386b8b5d 122
bbbobbbieo 15:830209e846d5 123 // value of center of black (single line)
bbbobbbieo 14:f43b386b8b5d 124 black_center_value= sum_black / black_value_count;
bbbobbbieo 15:830209e846d5 125
bbbobbbieo 14:f43b386b8b5d 126
bbbobbbieo 15:830209e846d5 127 // need to turn left
bbbobbbieo 15:830209e846d5 128 if (black_center_value < 64) {
bbbobbbieo 15:830209e846d5 129
bbbobbbieo 16:11ba5d6f42ba 130 current_servo_position= float(.01875*black_center_value-(1.2));
bbbobbbieo 14:f43b386b8b5d 131 if(current_servo_position <= -0.4)
bbbobbbieo 14:f43b386b8b5d 132 current_servo_position = -0.4;
bbbobbbieo 14:f43b386b8b5d 133 TFC_SetServo(0, current_servo_position);
bbbobbbieo 15:830209e846d5 134
bbbobbbieo 15:830209e846d5 135
bbbobbbieo 16:11ba5d6f42ba 136 //current_left_motor_speed = current_left_motor_speed + float(64-black_center_value)*.0025;
bbbobbbieo 16:11ba5d6f42ba 137 //current_right_motor_speed = current_right_motor_speed + float(64-black_center_value)*.0025;
bbbobbbieo 16:11ba5d6f42ba 138 if (violence_level !=0){
bbbobbbieo 18:3988310cd03b 139 current_left_motor_speed = current_left_motor_speed + float(float(64-black_center_value)*.025);// kinda reverse this...
bbbobbbieo 16:11ba5d6f42ba 140 current_right_motor_speed = current_right_motor_speed + float(float(64-black_center_value)*.045);// push more forwards
bbbobbbieo 16:11ba5d6f42ba 141 }
bbbobbbieo 16:11ba5d6f42ba 142
bbbobbbieo 16:11ba5d6f42ba 143 // protection block
bbbobbbieo 16:11ba5d6f42ba 144 if(current_left_motor_speed >= 0.5)
bbbobbbieo 16:11ba5d6f42ba 145 current_left_motor_speed= 0.5;
bbbobbbieo 16:11ba5d6f42ba 146 if(current_right_motor_speed >= 0.5)
bbbobbbieo 16:11ba5d6f42ba 147 current_right_motor_speed= 0.5;
bbbobbbieo 16:11ba5d6f42ba 148 if(current_left_motor_speed <= -0.5)
bbbobbbieo 16:11ba5d6f42ba 149 current_left_motor_speed= -0.5;
bbbobbbieo 16:11ba5d6f42ba 150 if(current_right_motor_speed <= -0.5)
bbbobbbieo 16:11ba5d6f42ba 151 current_right_motor_speed= -0.5;
bbbobbbieo 16:11ba5d6f42ba 152
bbbobbbieo 14:f43b386b8b5d 153 TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
bbbobbbieo 14:f43b386b8b5d 154
bbbobbbieo 7:455e7dd338ee 155 }
bbbobbbieo 14:f43b386b8b5d 156
bbbobbbieo 15:830209e846d5 157 // need to turn right
bbbobbbieo 14:f43b386b8b5d 158 if (black_center_value > 64) {
bbbobbbieo 14:f43b386b8b5d 159
bbbobbbieo 16:11ba5d6f42ba 160 current_servo_position= float(.01875*black_center_value-(1.2));
bbbobbbieo 14:f43b386b8b5d 161 if( current_servo_position >= +0.4)
bbbobbbieo 14:f43b386b8b5d 162 current_servo_position = +0.4;
bbbobbbieo 14:f43b386b8b5d 163 TFC_SetServo(0, current_servo_position);
bbbobbbieo 15:830209e846d5 164
bbbobbbieo 16:11ba5d6f42ba 165 //current_left_motor_speed = current_left_motor_speed - float(black_center_value-64)*.0025;
bbbobbbieo 16:11ba5d6f42ba 166 //current_right_motor_speed = current_right_motor_speed - float(black_center_value-64)*.0025;
bbbobbbieo 16:11ba5d6f42ba 167 if (violence_level !=0){
bbbobbbieo 16:11ba5d6f42ba 168 current_left_motor_speed = current_left_motor_speed - float(float(black_center_value-64)*.045);// push more forwards
bbbobbbieo 18:3988310cd03b 169 current_right_motor_speed = current_right_motor_speed - float(float(black_center_value-64)*.025);// kinda reverse this...
bbbobbbieo 16:11ba5d6f42ba 170 }
bbbobbbieo 15:830209e846d5 171
bbbobbbieo 16:11ba5d6f42ba 172 // protection block
bbbobbbieo 16:11ba5d6f42ba 173 if(current_left_motor_speed >= 0.5)
bbbobbbieo 16:11ba5d6f42ba 174 current_left_motor_speed= 0.5;
bbbobbbieo 16:11ba5d6f42ba 175 if(current_right_motor_speed >= 0.5)
bbbobbbieo 16:11ba5d6f42ba 176 current_right_motor_speed= 0.5;
bbbobbbieo 16:11ba5d6f42ba 177 if(current_left_motor_speed <= -0.5)
bbbobbbieo 16:11ba5d6f42ba 178 current_left_motor_speed= -0.5;
bbbobbbieo 16:11ba5d6f42ba 179 if(current_right_motor_speed <= -0.5)
bbbobbbieo 16:11ba5d6f42ba 180 current_right_motor_speed= -0.5;
bbbobbbieo 16:11ba5d6f42ba 181
bbbobbbieo 14:f43b386b8b5d 182 TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
bbbobbbieo 15:830209e846d5 183
bbbobbbieo 14:f43b386b8b5d 184 }
bbbobbbieo 14:f43b386b8b5d 185
bbbobbbieo 15:830209e846d5 186 // clearing values for next image processing round
bbbobbbieo 14:f43b386b8b5d 187 black_value_count = 0;
bbbobbbieo 14:f43b386b8b5d 188 black_center_value = 0;
bbbobbbieo 14:f43b386b8b5d 189 sum_black = 0;
bbbobbbieo 14:f43b386b8b5d 190
bbbobbbieo 15:830209e846d5 191 // end image processing
bbbobbbieo 14:f43b386b8b5d 192
bbbobbbieo 14:f43b386b8b5d 193 linescan_ping_pong = false;
bbbobbbieo 14:f43b386b8b5d 194 } // end checking channel 0
bbbobbbieo 15:830209e846d5 195
bbbobbbieo 15:830209e846d5 196 else { //checking channel 1
bbbobbbieo 14:f43b386b8b5d 197 linescan_ping_pong = true;
bbbobbbieo 14:f43b386b8b5d 198 }
bbbobbbieo 14:f43b386b8b5d 199
bbbobbbieo 7:455e7dd338ee 200 TFC_LineScanImageReady ==0; // since we used it, we reset the flag
bbbobbbieo 14:f43b386b8b5d 201 }// end imageready
bbbobbbieo 14:f43b386b8b5d 202 }// end linescan stuff
bbbobbbieo 0:d57117b2188d 203 }
bbbobbbieo 0:d57117b2188d 204 }
bbbobbbieo 8:946806df7347 205
bbbobbbieo 15:830209e846d5 206 // shit code down here