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

newAlgorithm.cpp@43:9f541718be01, 2015-03-10 (annotated)

Committer:
mperella
Date:
Tue Mar 10 16:57:23 2015 +0000
Revision:
43:9f541718be01
Parent:
42:c189b914931d 
think the linescan stuff might be right

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mperella 40:cdb079fc9d0a 1 #include "TFC.h"
mperella 40:cdb079fc9d0a 2 #include <iostream>
mperella 40:cdb079fc9d0a 3 #include <stdio.h>
mperella 40:cdb079fc9d0a 4
mperella 40:cdb079fc9d0a 5 const float proportional = 0;
mperella 40:cdb079fc9d0a 6 const float last_proportional = 0;
mperella 40:cdb079fc9d0a 7 const float integral = 0;
mperella 40:cdb079fc9d0a 8 const float derivative = 0;
mperella 40:cdb079fc9d0a 9 const float output = 0;
mperella 40:cdb079fc9d0a 10
mperella 41:79ccb6f70ab6 11 // these are definitely not right
mperella 40:cdb079fc9d0a 12 const float kp = 0.05;
mperella 40:cdb079fc9d0a 13 const float ki = 0.02;
mperella 40:cdb079fc9d0a 14 const float kd = 0.02;
mperella 40:cdb079fc9d0a 15
mperella 40:cdb079fc9d0a 16 const int set_point = 63;
mperella 40:cdb079fc9d0a 17 const int position = 0;
mperella 40:cdb079fc9d0a 18 const int error = 0;
mperella 40:cdb079fc9d0a 19
mperella 40:cdb079fc9d0a 20 const float AGGRESSIVE = .60;
mperella 40:cdb079fc9d0a 21 const float MODERATE =.45;
mperella 40:cdb079fc9d0a 22 const float CONSERVATIVE =.30;
mperella 40:cdb079fc9d0a 23 const float STOP =0;
mperella 40:cdb079fc9d0a 24
mperella 42:c189b914931d 25 const bool linescan_enable = true;
mperella 42:c189b914931d 26 const bool image = true;
mperella 42:c189b914931d 27
mperella 40:cdb079fc9d0a 28 const int black_values_list[LINE_SCAN_LENGTH];
mperella 40:cdb079fc9d0a 29 const int black_value_count = 0;
mperella 40:cdb079fc9d0a 30 const int black_center_value = 0;
mperella 40:cdb079fc9d0a 31 const int sum_black = 0;
mperella 40:cdb079fc9d0a 32 const int violence_level = 0;
mperella 40:cdb079fc9d0a 33
mperella 40:cdb079fc9d0a 34 const float current_left_motor_speed = 0;
mperella 40:cdb079fc9d0a 35 const float current_right_motor_speed = 0;
mperella 40:cdb079fc9d0a 36
mperella 40:cdb079fc9d0a 37 const float current_right_max = 0;
mperella 40:cdb079fc9d0a 38 const float current_left_max = 0;
mperella 40:cdb079fc9d0a 39
mperella 40:cdb079fc9d0a 40
mperella 40:cdb079fc9d0a 41 int main()
mperella 40:cdb079fc9d0a 42 {
mperella 40:cdb079fc9d0a 43 TFC_Init();
mperella 40:cdb079fc9d0a 44 if(rear_motor_enable_flag)
mperella 40:cdb079fc9d0a 45 {
mperella 40:cdb079fc9d0a 46 TFC_HBRIDGE_ENABLE;
mperella 40:cdb079fc9d0a 47
mperella 40:cdb079fc9d0a 48 //current_left_motor_speed = (TFC_ReadPot(0));
mperella 40:cdb079fc9d0a 49 //current_right_motor_speed = (TFC_ReadPot(1));
mperella 40:cdb079fc9d0a 50
mperella 40:cdb079fc9d0a 51 // checking behavior level
mperella 40:cdb079fc9d0a 52 violence_level = int(TFC_GetDIP_Switch());
mperella 40:cdb079fc9d0a 53
mperella 40:cdb079fc9d0a 54 if (violence_level==3) {
mperella 40:cdb079fc9d0a 55 current_left_motor_speed = -(AGGRESSIVE);
mperella 40:cdb079fc9d0a 56 current_right_motor_speed = AGGRESSIVE;
mperella 40:cdb079fc9d0a 57 current_right_max = AGGRESSIVE;
mperella 40:cdb079fc9d0a 58 current_left_max = -AGGRESSIVE;
mperella 40:cdb079fc9d0a 59
mperella 40:cdb079fc9d0a 60 }
mperella 40:cdb079fc9d0a 61 else if (violence_level==2) {
mperella 40:cdb079fc9d0a 62 current_left_motor_speed = -(MODERATE);
mperella 40:cdb079fc9d0a 63 current_right_motor_speed = (MODERATE);
mperella 40:cdb079fc9d0a 64 current_right_max = MODERATE;
mperella 40:cdb079fc9d0a 65 current_left_max = -MODERATE;
mperella 40:cdb079fc9d0a 66 Kp = Kp * 0.75;
mperella 40:cdb079fc9d0a 67 Ki = Ki * 0.75;
mperella 40:cdb079fc9d0a 68 Kd = Kd * 0.75;
mperella 40:cdb079fc9d0a 69 }
mperella 40:cdb079fc9d0a 70 else if (violence_level==1) {
mperella 40:cdb079fc9d0a 71 current_left_motor_speed = -(CONSERVATIVE);
mperella 40:cdb079fc9d0a 72 current_right_motor_speed = CONSERVATIVE;
mperella 40:cdb079fc9d0a 73 current_right_max = CONSERVATIVE;
mperella 40:cdb079fc9d0a 74 current_left_max = CONSERVATIVE;
mperella 40:cdb079fc9d0a 75 Kp = Kp * 0.5;
mperella 40:cdb079fc9d0a 76 Ki = Ki * 0.5;
mperella 40:cdb079fc9d0a 77 Kd = Kd * 0.5;
mperella 40:cdb079fc9d0a 78 }
mperella 40:cdb079fc9d0a 79 else if (violence_level==0) {
mperella 40:cdb079fc9d0a 80 current_left_motor_speed = STOP;
mperella 40:cdb079fc9d0a 81 current_right_motor_speed = STOP;
mperella 40:cdb079fc9d0a 82 }
mperella 40:cdb079fc9d0a 83 else {
mperella 40:cdb079fc9d0a 84 current_left_motor_speed = STOP;
mperella 40:cdb079fc9d0a 85 current_right_motor_speed = STOP;
mperella 40:cdb079fc9d0a 86 }
mperella 40:cdb079fc9d0a 87
mperella 40:cdb079fc9d0a 88
mperella 40:cdb079fc9d0a 89 // protection block
mperella 40:cdb079fc9d0a 90 /*
mperella 40:cdb079fc9d0a 91 if(current_left_motor_speed >= PROTECTION_THRESHOLD_UPPER)
mperella 40:cdb079fc9d0a 92 current_left_motor_speed= PROTECTION_THRESHOLD_UPPER;
mperella 40:cdb079fc9d0a 93 if(current_right_motor_speed >= PROTECTION_THRESHOLD_UPPER)
mperella 40:cdb079fc9d0a 94 current_right_motor_speed = PROTECTION_THRESHOLD_UPPER;
mperella 40:cdb079fc9d0a 95 if(current_left_motor_speed <= PROTECTION_THRESHOLD_LOWER)
mperella 40:cdb079fc9d0a 96 current_left_motor_speed = PROTECTION_THRESHOLD_LOWER;
mperella 40:cdb079fc9d0a 97 if(current_right_motor_speed <= PROTECTION_THRESHOLD_LOWER)
mperella 40:cdb079fc9d0a 98 current_right_motor_speed = PROTECTION_THRESHOLD_LOWER;
mperella 40:cdb079fc9d0a 99 */
mperella 40:cdb079fc9d0a 100
mperella 40:cdb079fc9d0a 101 TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
mperella 40:cdb079fc9d0a 102 }// end motor enabled
mperella 40:cdb079fc9d0a 103 else {
mperella 40:cdb079fc9d0a 104 TFC_HBRIDGE_DISABLE;
mperella 40:cdb079fc9d0a 105 }// end motor disabled
mperella 40:cdb079fc9d0a 106
mperella 40:cdb079fc9d0a 107 }
mperella 40:cdb079fc9d0a 108
mperella 40:cdb079fc9d0a 109 void loop()
mperella 40:cdb079fc9d0a 110 {
mperella 40:cdb079fc9d0a 111 if (linescan_enable)
mperella 40:cdb079fc9d0a 112 {
mperella 40:cdb079fc9d0a 113 if (TFC_LineScanImageReady !=0)
mperella 40:cdb079fc9d0a 114 {
mperella 42:c189b914931d 115 // first image
mperella 43:9f541718be01 116 if(CurrentLineScanChannel)
mperella 43:9f541718be01 117 {
mperella 43:9f541718be01 118 position = getSensorValues(true); // get position
mperella 43:9f541718be01 119 }
mperella 43:9f541718be01 120 // second image
mperella 43:9f541718be01 121 else
mperella 43:9f541718be01 122 {
mperella 43:9f541718be01 123 position = getSensorValues(false);
mperella 43:9f541718be01 124 }
mperella 42:c189b914931d 125 error = pid_calc(position); // send position to PID algorithm to get error
mperella 42:c189b914931d 126 calcMotors(error); // send error to calculate both motors
mperella 40:cdb079fc9d0a 127 }
mperella 40:cdb079fc9d0a 128 }
mperella 40:cdb079fc9d0a 129 else
mperella 40:cdb079fc9d0a 130 {
mperella 40:cdb079fc9d0a 131 TFC_SetMotorPWM(STOP, STOP);
mperella 40:cdb079fc9d0a 132 }
mperella 40:cdb079fc9d0a 133 }
mperella 40:cdb079fc9d0a 134
mperella 42:c189b914931d 135 int getSensorValues(bool image)
mperella 40:cdb079fc9d0a 136 {
mperella 42:c189b914931d 137 if(image)
mperella 40:cdb079fc9d0a 138 {
mperella 42:c189b914931d 139 for (uint16_t i=0; i<128; i++)
mperella 40:cdb079fc9d0a 140 {
mperella 43:9f541718be01 141 if ((*(TFC_LineScanImage0+i) < 250))
mperella 42:c189b914931d 142 {
mperella 42:c189b914931d 143 black_values_list[black_value_count] = i;
mperella 42:c189b914931d 144 black_value_count++;
mperella 42:c189b914931d 145 }
mperella 40:cdb079fc9d0a 146 }
mperella 42:c189b914931d 147 }
mperella 42:c189b914931d 148 else
mperella 42:c189b914931d 149 {
mperella 42:c189b914931d 150 for (uint16_t i=0; i<128; i++)
mperella 42:c189b914931d 151 {
mperella 43:9f541718be01 152 if ((*(TFC_LineScanImage1+i) < 250))
mperella 42:c189b914931d 153 {
mperella 42:c189b914931d 154 black_values_list[black_value_count] = i;
mperella 42:c189b914931d 155 black_value_count++;
mperella 42:c189b914931d 156 }
mperella 42:c189b914931d 157 }
mperella 40:cdb079fc9d0a 158 }
mperella 40:cdb079fc9d0a 159
mperella 40:cdb079fc9d0a 160 for(int i=0; i<black_value_count; i++)
mperella 40:cdb079fc9d0a 161 {
mperella 40:cdb079fc9d0a 162 sum_black += black_values_list[i];
mperella 40:cdb079fc9d0a 163 }
mperella 40:cdb079fc9d0a 164
mperella 42:c189b914931d 165 black_center_value = int(sum_black / black_value_count);
mperella 42:c189b914931d 166 return black_center_value;
mperella 42:c189b914931d 167
mperella 40:cdb079fc9d0a 168 }
mperella 40:cdb079fc9d0a 169
mperella 40:cdb079fc9d0a 170 int pid_calc(int pos)
mperella 40:cdb079fc9d0a 171 {
mperella 40:cdb079fc9d0a 172 position = pos;
mperella 40:cdb079fc9d0a 173 proportional = position - set_point;
mperella 40:cdb079fc9d0a 174 integral = integral + proportional;
mperella 40:cdb079fc9d0a 175 derivative= proportional - last_proportional;
mperella 40:cdb079fc9d0a 176 last_proportional = proportional;
mperella 40:cdb079fc9d0a 177 error = proportional*Kp + integral*Ki + derivative*Kd;
mperella 41:79ccb6f70ab6 178 return error;
mperella 40:cdb079fc9d0a 179 }
mperella 40:cdb079fc9d0a 180
mperella 40:cdb079fc9d0a 181 void calcMotors(float error)
mperella 40:cdb079fc9d0a 182 {
mperella 40:cdb079fc9d0a 183 TFC_SetServo(0, error);
mperella 40:cdb079fc9d0a 184
mperella 40:cdb079fc9d0a 185 if(error < 0)
mperella 40:cdb079fc9d0a 186 {
mperella 40:cdb079fc9d0a 187 current_left_motor_speed = current_left_motor_speed + error; // reverse this
mperella 40:cdb079fc9d0a 188
mperella 40:cdb079fc9d0a 189 // make sure not going too fast
mperella 40:cdb079fc9d0a 190 if(current_right_motor_speed > current_right_max)
mperella 40:cdb079fc9d0a 191 {
mperella 40:cdb079fc9d0a 192 current_right_motor_speed = current_right_max;
mperella 40:cdb079fc9d0a 193 }
mperella 40:cdb079fc9d0a 194 else
mperella 40:cdb079fc9d0a 195 {
mperella 40:cdb079fc9d0a 196 current_right_motor_speed = current_right_motor_speed + error;
mperella 40:cdb079fc9d0a 197 }
mperella 40:cdb079fc9d0a 198 }
mperella 40:cdb079fc9d0a 199 else
mperella 40:cdb079fc9d0a 200 {
mperella 40:cdb079fc9d0a 201 current_right_motor_speed = current_right_motor_speed - error;
mperella 40:cdb079fc9d0a 202
mperella 40:cdb079fc9d0a 203 // make sure not going too fast
mperella 40:cdb079fc9d0a 204 if(current_left_motor_speed < current_left_max)
mperella 40:cdb079fc9d0a 205 {
mperella 40:cdb079fc9d0a 206 current_left_motor_speed = current_left_max;
mperella 40:cdb079fc9d0a 207 }
mperella 40:cdb079fc9d0a 208 else
mperella 40:cdb079fc9d0a 209 {
mperella 40:cdb079fc9d0a 210 current_left_motor_speed = current_left_motor_speed - error;
mperella 40:cdb079fc9d0a 211 }
mperella 40:cdb079fc9d0a 212 }
mperella 40:cdb079fc9d0a 213
mperella 40:cdb079fc9d0a 214 TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
mperella 40:cdb079fc9d0a 215 }