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@17:c643b6b4a96f, 2015-02-25 (annotated)

Committer:
mperella
Date:
Wed Feb 25 19:35:14 2015 +0000
Revision:
17:c643b6b4a96f
Parent:
16:11ba5d6f42ba
Child:
19:85eb7991e2ab
added some initial pid stuff

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;
mperella 17:c643b6b4a96f 15
mperella 17:c643b6b4a96f 16 float dt = 0.00001;
mperella 17:c643b6b4a96f 17 float integral = 0;
mperella 17:c643b6b4a96f 18 float derivative = 0;
mperella 17:c643b6b4a96f 19 float output = 0;
mperella 17:c643b6b4a96f 20
mperella 17:c643b6b4a96f 21 // gains on prop, int, der
mperella 17:c643b6b4a96f 22 // subject to change, need to fine tune
mperella 17:c643b6b4a96f 23 float kp = 1.8960;
mperella 17:c643b6b4a96f 24 float ki = 0.6170;
mperella 17:c643b6b4a96f 25 float kd = 1.5590;
mperella 17:c643b6b4a96f 26
bbbobbbieo 6:44d1079f076c 27 bool rear_motor_enable_flag = true;
bbbobbbieo 7:455e7dd338ee 28 bool linescan_ping_pong = false;
bbbobbbieo 9:2b028ee421ad 29 bool linescan_enable = true;
bbbobbbieo 14:f43b386b8b5d 30
bbbobbbieo 11:d65d6d7fc85e 31 int black_values_list[128];
bbbobbbieo 11:d65d6d7fc85e 32 int black_value_count = 0;
bbbobbbieo 11:d65d6d7fc85e 33 int black_center_value = 0;
bbbobbbieo 11:d65d6d7fc85e 34 int sum_black = 0;
bbbobbbieo 14:f43b386b8b5d 35 int violence_level = 0;
bbbobbbieo 15:830209e846d5 36
mperella 17:c643b6b4a96f 37 int set_point = 64;
mperella 17:c643b6b4a96f 38 int previous error = 0;
mperella 17:c643b6b4a96f 39 int error = 0;
mperella 17:c643b6b4a96f 40
mperella 17:c643b6b4a96f 41
bbbobbbieo 9:2b028ee421ad 42 //uint16_t MyImage0Buffer[2][128];
bbbobbbieo 9:2b028ee421ad 43 //uint16_t MyImage1Buffer[2][128];
bbbobbbieo 14:f43b386b8b5d 44
bbbobbbieo 7:455e7dd338ee 45 // major loop
bbbobbbieo 14:f43b386b8b5d 46 while(1) {
bbbobbbieo 14:f43b386b8b5d 47
bbbobbbieo 15:830209e846d5 48 // manual servo control, unused
bbbobbbieo 14:f43b386b8b5d 49 if (TFC_ReadPushButton(0) != 0 ) {
bbbobbbieo 14:f43b386b8b5d 50 current_servo_position = current_servo_position-.005;
bbbobbbieo 14:f43b386b8b5d 51 if(current_servo_position <= -0.4)
bbbobbbieo 14:f43b386b8b5d 52 current_servo_position = -0.4;
bbbobbbieo 14:f43b386b8b5d 53 TFC_SetServo(0, current_servo_position);
bbbobbbieo 14:f43b386b8b5d 54 }// end check button0
bbbobbbieo 14:f43b386b8b5d 55
bbbobbbieo 15:830209e846d5 56 else {}
bbbobbbieo 14:f43b386b8b5d 57
bbbobbbieo 15:830209e846d5 58 // manual servo control, unused
bbbobbbieo 14:f43b386b8b5d 59 if (TFC_ReadPushButton(1) != 0 ) {
bbbobbbieo 14:f43b386b8b5d 60 current_servo_position = current_servo_position+.005;
bbbobbbieo 14:f43b386b8b5d 61 if(current_servo_position >= 0.4)
bbbobbbieo 14:f43b386b8b5d 62 current_servo_position = 0.4;
bbbobbbieo 14:f43b386b8b5d 63 TFC_SetServo(0, current_servo_position);
bbbobbbieo 14:f43b386b8b5d 64 }// end check button1
bbbobbbieo 14:f43b386b8b5d 65
bbbobbbieo 15:830209e846d5 66 else {}
bbbobbbieo 14:f43b386b8b5d 67
bbbobbbieo 15:830209e846d5 68 // initial motor stuff
bbbobbbieo 14:f43b386b8b5d 69 if(rear_motor_enable_flag) {
bbbobbbieo 14:f43b386b8b5d 70 TFC_HBRIDGE_ENABLE;
bbbobbbieo 14:f43b386b8b5d 71
bbbobbbieo 15:830209e846d5 72 //current_left_motor_speed = (TFC_ReadPot(0));
bbbobbbieo 15:830209e846d5 73 //current_right_motor_speed = (TFC_ReadPot(1));
bbbobbbieo 14:f43b386b8b5d 74
bbbobbbieo 15:830209e846d5 75 // checking behavior level
bbbobbbieo 14:f43b386b8b5d 76 violence_level = int(TFC_GetDIP_Switch());
bbbobbbieo 14:f43b386b8b5d 77
bbbobbbieo 15:830209e846d5 78 if (violence_level==2) {
bbbobbbieo 16:11ba5d6f42ba 79 current_left_motor_speed = -.48;
bbbobbbieo 16:11ba5d6f42ba 80 current_right_motor_speed = .48;
bbbobbbieo 15:830209e846d5 81 }
bbbobbbieo 15:830209e846d5 82 else if (violence_level==1) {
bbbobbbieo 16:11ba5d6f42ba 83 current_left_motor_speed = -.35;
bbbobbbieo 16:11ba5d6f42ba 84 current_right_motor_speed = .35;
bbbobbbieo 14:f43b386b8b5d 85 }
bbbobbbieo 15:830209e846d5 86 else if (violence_level==0) {
bbbobbbieo 15:830209e846d5 87 current_left_motor_speed = 0;
bbbobbbieo 15:830209e846d5 88 current_right_motor_speed = 0;
bbbobbbieo 15:830209e846d5 89 }
bbbobbbieo 15:830209e846d5 90 else {
bbbobbbieo 15:830209e846d5 91 current_left_motor_speed = 0;
bbbobbbieo 15:830209e846d5 92 current_right_motor_speed = 0;
bbbobbbieo 15:830209e846d5 93 }
bbbobbbieo 15:830209e846d5 94
bbbobbbieo 14:f43b386b8b5d 95
bbbobbbieo 15:830209e846d5 96 // protection block
bbbobbbieo 15:830209e846d5 97 if(current_left_motor_speed >= 0.5)
bbbobbbieo 15:830209e846d5 98 current_left_motor_speed= 0.5;
bbbobbbieo 15:830209e846d5 99 if(current_right_motor_speed >= 0.5)
bbbobbbieo 15:830209e846d5 100 current_right_motor_speed= 0.5;
bbbobbbieo 15:830209e846d5 101 if(current_left_motor_speed <= -0.5)
bbbobbbieo 15:830209e846d5 102 current_left_motor_speed= -0.5;
bbbobbbieo 15:830209e846d5 103 if(current_right_motor_speed <= -0.5)
bbbobbbieo 15:830209e846d5 104 current_right_motor_speed= -0.5;
bbbobbbieo 14:f43b386b8b5d 105
bbbobbbieo 14:f43b386b8b5d 106 TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
bbbobbbieo 14:f43b386b8b5d 107 }// end motor enabled
bbbobbbieo 14:f43b386b8b5d 108 else {
bbbobbbieo 14:f43b386b8b5d 109 TFC_HBRIDGE_DISABLE;
bbbobbbieo 14:f43b386b8b5d 110 }// end motor disabled
bbbobbbieo 14:f43b386b8b5d 111
bbbobbbieo 15:830209e846d5 112 // camera stuff
bbbobbbieo 14:f43b386b8b5d 113 if (linescan_enable) {
bbbobbbieo 14:f43b386b8b5d 114 if (TFC_LineScanImageReady !=0) {
bbbobbbieo 14:f43b386b8b5d 115
bbbobbbieo 15:830209e846d5 116 if (linescan_ping_pong) {
bbbobbbieo 15:830209e846d5 117 //checking channel 0
bbbobbbieo 15:830209e846d5 118
bbbobbbieo 15:830209e846d5 119 //checking center pixel, displays aprox value on leds
bbbobbbieo 14:f43b386b8b5d 120 uint8_t shitnum = 1;
bbbobbbieo 14:f43b386b8b5d 121 if (*(TFC_LineScanImage0+64) > 800)
bbbobbbieo 14:f43b386b8b5d 122 shitnum = 15;
bbbobbbieo 14:f43b386b8b5d 123 else if((*(TFC_LineScanImage0+64) > 450))
bbbobbbieo 14:f43b386b8b5d 124 shitnum = 7;
bbbobbbieo 14:f43b386b8b5d 125 else if((*(TFC_LineScanImage0+64) > 400))
bbbobbbieo 14:f43b386b8b5d 126 shitnum = 3;
bbbobbbieo 14:f43b386b8b5d 127 else
bbbobbbieo 14:f43b386b8b5d 128 shitnum = 1;
bbbobbbieo 14:f43b386b8b5d 129 TFC_SetBatteryLED(shitnum);
bbbobbbieo 14:f43b386b8b5d 130
bbbobbbieo 15:830209e846d5 131
bbbobbbieo 15:830209e846d5 132 // checking for center line (single line)
bbbobbbieo 14:f43b386b8b5d 133 for (uint16_t i=0; i<128; i++) {
bbbobbbieo 14:f43b386b8b5d 134 if ((*(TFC_LineScanImage0+i) < 300)) {
bbbobbbieo 14:f43b386b8b5d 135 black_values_list[black_value_count] = i;
bbbobbbieo 14:f43b386b8b5d 136 black_value_count++;
bbbobbbieo 14:f43b386b8b5d 137 }
bbbobbbieo 14:f43b386b8b5d 138 }
bbbobbbieo 14:f43b386b8b5d 139
bbbobbbieo 14:f43b386b8b5d 140 for(int i=0; i<black_value_count; i++) {
bbbobbbieo 14:f43b386b8b5d 141 sum_black += black_values_list[i];
bbbobbbieo 14:f43b386b8b5d 142 }
bbbobbbieo 14:f43b386b8b5d 143
bbbobbbieo 15:830209e846d5 144 // value of center of black (single line)
bbbobbbieo 14:f43b386b8b5d 145 black_center_value= sum_black / black_value_count;
bbbobbbieo 15:830209e846d5 146
mperella 17:c643b6b4a96f 147 /* ******* PID ALGORITHM *******
mperella 17:c643b6b4a96f 148
mperella 17:c643b6b4a96f 149 error = set_point - black_center_value;
mperella 17:c643b6b4a96f 150 integral = integral + error*dt;
mperella 17:c643b6b4a96f 151 derivative = (error - previous_error)/dt;
mperella 17:c643b6b4a96f 152 output = kp*error + ki*integral + kd*derivative;
mperella 17:c643b6b4a96f 153 previous error = error;
mperella 17:c643b6b4a96f 154
mperella 17:c643b6b4a96f 155 GOTTA DO SOME TESTS/SIMULATIONS TO CALIBRATE SERVO ADJUSTMENTS
mperella 17:c643b6b4a96f 156
mperella 17:c643b6b4a96f 157
mperella 17:c643b6b4a96f 158 *****************************
mperella 17:c643b6b4a96f 159 */
bbbobbbieo 14:f43b386b8b5d 160
bbbobbbieo 15:830209e846d5 161 // need to turn left
bbbobbbieo 15:830209e846d5 162 if (black_center_value < 64) {
bbbobbbieo 15:830209e846d5 163
bbbobbbieo 16:11ba5d6f42ba 164 current_servo_position= float(.01875*black_center_value-(1.2));
bbbobbbieo 14:f43b386b8b5d 165 if(current_servo_position <= -0.4)
bbbobbbieo 14:f43b386b8b5d 166 current_servo_position = -0.4;
bbbobbbieo 14:f43b386b8b5d 167 TFC_SetServo(0, current_servo_position);
bbbobbbieo 15:830209e846d5 168
bbbobbbieo 15:830209e846d5 169
bbbobbbieo 16:11ba5d6f42ba 170 //current_left_motor_speed = current_left_motor_speed + float(64-black_center_value)*.0025;
bbbobbbieo 16:11ba5d6f42ba 171 //current_right_motor_speed = current_right_motor_speed + float(64-black_center_value)*.0025;
bbbobbbieo 16:11ba5d6f42ba 172 if (violence_level !=0){
bbbobbbieo 16:11ba5d6f42ba 173 current_left_motor_speed = current_left_motor_speed + float(float(64-black_center_value)*.045);// kinda reverse this...
bbbobbbieo 16:11ba5d6f42ba 174 current_right_motor_speed = current_right_motor_speed + float(float(64-black_center_value)*.045);// push more forwards
bbbobbbieo 16:11ba5d6f42ba 175 }
bbbobbbieo 16:11ba5d6f42ba 176
bbbobbbieo 16:11ba5d6f42ba 177 // protection block
bbbobbbieo 16:11ba5d6f42ba 178 if(current_left_motor_speed >= 0.5)
bbbobbbieo 16:11ba5d6f42ba 179 current_left_motor_speed= 0.5;
bbbobbbieo 16:11ba5d6f42ba 180 if(current_right_motor_speed >= 0.5)
bbbobbbieo 16:11ba5d6f42ba 181 current_right_motor_speed= 0.5;
bbbobbbieo 16:11ba5d6f42ba 182 if(current_left_motor_speed <= -0.5)
bbbobbbieo 16:11ba5d6f42ba 183 current_left_motor_speed= -0.5;
bbbobbbieo 16:11ba5d6f42ba 184 if(current_right_motor_speed <= -0.5)
bbbobbbieo 16:11ba5d6f42ba 185 current_right_motor_speed= -0.5;
bbbobbbieo 16:11ba5d6f42ba 186
bbbobbbieo 14:f43b386b8b5d 187 TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
bbbobbbieo 14:f43b386b8b5d 188
bbbobbbieo 7:455e7dd338ee 189 }
bbbobbbieo 14:f43b386b8b5d 190
bbbobbbieo 15:830209e846d5 191 // need to turn right
bbbobbbieo 14:f43b386b8b5d 192 if (black_center_value > 64) {
bbbobbbieo 14:f43b386b8b5d 193
bbbobbbieo 16:11ba5d6f42ba 194 current_servo_position= float(.01875*black_center_value-(1.2));
bbbobbbieo 14:f43b386b8b5d 195 if( current_servo_position >= +0.4)
bbbobbbieo 14:f43b386b8b5d 196 current_servo_position = +0.4;
bbbobbbieo 14:f43b386b8b5d 197 TFC_SetServo(0, current_servo_position);
bbbobbbieo 15:830209e846d5 198
bbbobbbieo 16:11ba5d6f42ba 199 //current_left_motor_speed = current_left_motor_speed - float(black_center_value-64)*.0025;
bbbobbbieo 16:11ba5d6f42ba 200 //current_right_motor_speed = current_right_motor_speed - float(black_center_value-64)*.0025;
bbbobbbieo 16:11ba5d6f42ba 201 if (violence_level !=0){
bbbobbbieo 16:11ba5d6f42ba 202 current_left_motor_speed = current_left_motor_speed - float(float(black_center_value-64)*.045);// push more forwards
bbbobbbieo 16:11ba5d6f42ba 203 current_right_motor_speed = current_right_motor_speed - float(float(black_center_value-64)*.045);// kinda reverse this...
bbbobbbieo 16:11ba5d6f42ba 204 }
bbbobbbieo 15:830209e846d5 205
bbbobbbieo 16:11ba5d6f42ba 206 // protection block
bbbobbbieo 16:11ba5d6f42ba 207 if(current_left_motor_speed >= 0.5)
bbbobbbieo 16:11ba5d6f42ba 208 current_left_motor_speed= 0.5;
bbbobbbieo 16:11ba5d6f42ba 209 if(current_right_motor_speed >= 0.5)
bbbobbbieo 16:11ba5d6f42ba 210 current_right_motor_speed= 0.5;
bbbobbbieo 16:11ba5d6f42ba 211 if(current_left_motor_speed <= -0.5)
bbbobbbieo 16:11ba5d6f42ba 212 current_left_motor_speed= -0.5;
bbbobbbieo 16:11ba5d6f42ba 213 if(current_right_motor_speed <= -0.5)
bbbobbbieo 16:11ba5d6f42ba 214 current_right_motor_speed= -0.5;
bbbobbbieo 16:11ba5d6f42ba 215
bbbobbbieo 14:f43b386b8b5d 216 TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
bbbobbbieo 15:830209e846d5 217
bbbobbbieo 14:f43b386b8b5d 218 }
bbbobbbieo 14:f43b386b8b5d 219
bbbobbbieo 15:830209e846d5 220 // clearing values for next image processing round
bbbobbbieo 14:f43b386b8b5d 221 black_value_count = 0;
bbbobbbieo 14:f43b386b8b5d 222 black_center_value = 0;
bbbobbbieo 14:f43b386b8b5d 223 sum_black = 0;
bbbobbbieo 14:f43b386b8b5d 224
bbbobbbieo 15:830209e846d5 225 // end image processing
bbbobbbieo 14:f43b386b8b5d 226
bbbobbbieo 14:f43b386b8b5d 227 linescan_ping_pong = false;
bbbobbbieo 14:f43b386b8b5d 228 } // end checking channel 0
bbbobbbieo 15:830209e846d5 229
bbbobbbieo 15:830209e846d5 230 else { //checking channel 1
bbbobbbieo 14:f43b386b8b5d 231 linescan_ping_pong = true;
bbbobbbieo 14:f43b386b8b5d 232 }
bbbobbbieo 14:f43b386b8b5d 233
bbbobbbieo 7:455e7dd338ee 234 TFC_LineScanImageReady ==0; // since we used it, we reset the flag
bbbobbbieo 14:f43b386b8b5d 235 }// end imageready
bbbobbbieo 14:f43b386b8b5d 236 }// end linescan stuff
bbbobbbieo 0:d57117b2188d 237 }
bbbobbbieo 0:d57117b2188d 238 }
bbbobbbieo 8:946806df7347 239
bbbobbbieo 8:946806df7347 240
bbbobbbieo 15:830209e846d5 241 // shit code down here