stephen smitherman / Mbed 2 deprecated TJPS

Robot project.

Dependencies:   HCSR04 PID QEI mbed

main.cpp@1:82463f25fd3c, 2013-12-12 (annotated)

Committer:
srsmitherman
Date:
Thu Dec 12 17:30:49 2013 +0000
Revision:
1:82463f25fd3c
Parent:
0:8a066434c28e 
Initial publish.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
srsmitherman 0:8a066434c28e 1 #include "mbed.h"
srsmitherman 0:8a066434c28e 2 #include "HCSR04.h"
srsmitherman 0:8a066434c28e 3 #include "DRV8833.h"
srsmitherman 0:8a066434c28e 4 #include "QEI.h"
srsmitherman 0:8a066434c28e 5 #include "PID.h"
srsmitherman 0:8a066434c28e 6 #include "stdio.h"
srsmitherman 0:8a066434c28e 7 #include "LPC17xx.h"
srsmitherman 0:8a066434c28e 8
srsmitherman 0:8a066434c28e 9
srsmitherman 0:8a066434c28e 10 #define PIN_TRIGGER (p12)
srsmitherman 0:8a066434c28e 11 #define PIN_ECHO (p11)
srsmitherman 0:8a066434c28e 12 #define PULSE_PER_REV (1192)
srsmitherman 0:8a066434c28e 13 #define WHEEL_CIRCUM (12.56637)
srsmitherman 0:8a066434c28e 14 #define DIST_PER_PULSE (0.01054225722682)
srsmitherman 0:8a066434c28e 15 #define MTRS_TO_INCH (39.3701)
srsmitherman 0:8a066434c28e 16 #define MAX_SPEED (0.8)
srsmitherman 0:8a066434c28e 17
srsmitherman 0:8a066434c28e 18
srsmitherman 0:8a066434c28e 19 //Hardware Initilization
srsmitherman 0:8a066434c28e 20 Serial bt(p28,p27);
srsmitherman 0:8a066434c28e 21 Serial pc(USBTX,USBRX);
srsmitherman 0:8a066434c28e 22 HCSR04 rangeFinder( PIN_TRIGGER, PIN_ECHO );
srsmitherman 0:8a066434c28e 23 DRV8833 leftMotors(p21,p22);
srsmitherman 0:8a066434c28e 24 DRV8833 rightMotors(p23,p24);
srsmitherman 0:8a066434c28e 25 //QEI leftEncoder(p5,p6,NC,1192,QEI::X4_ENCODING);
srsmitherman 0:8a066434c28e 26 //QEI rightEncoder(p7,p8,NC,1192,QEI::X4_ENCODING);
srsmitherman 0:8a066434c28e 27 PID pid1(1.0,0.0,0.0,0.1);
srsmitherman 0:8a066434c28e 28 InterruptIn encoder(p29);
srsmitherman 0:8a066434c28e 29
srsmitherman 0:8a066434c28e 30 //Prototypes
srsmitherman 0:8a066434c28e 31 float getUltraSonic(void);
srsmitherman 0:8a066434c28e 32 void UARTIntHandler(void);
srsmitherman 0:8a066434c28e 33 void start(void);
srsmitherman 0:8a066434c28e 34 void stop(void);
srsmitherman 0:8a066434c28e 35 void forward(void);
srsmitherman 0:8a066434c28e 36 void reverse(void);
srsmitherman 0:8a066434c28e 37 void left(void);
srsmitherman 0:8a066434c28e 38 void right(void);
srsmitherman 0:8a066434c28e 39 void us_distance(void);
srsmitherman 0:8a066434c28e 40 void wall_follow(void);
srsmitherman 0:8a066434c28e 41 void bad_command(void);
srsmitherman 0:8a066434c28e 42 void encoderIntHandler(void);
srsmitherman 0:8a066434c28e 43 void encoderReset(void);
srsmitherman 0:8a066434c28e 44 float encoderDistance(void);
srsmitherman 0:8a066434c28e 45 int command_int(char *command);
srsmitherman 0:8a066434c28e 46
srsmitherman 0:8a066434c28e 47
srsmitherman 0:8a066434c28e 48 //Variables
srsmitherman 0:8a066434c28e 49 const int num_commands = 9;
srsmitherman 0:8a066434c28e 50 float range, echo;
srsmitherman 0:8a066434c28e 51 char inputString[12];
srsmitherman 0:8a066434c28e 52 int commandNumber = 0;
srsmitherman 0:8a066434c28e 53 unsigned long temp1;
srsmitherman 0:8a066434c28e 54 unsigned long temp2;
srsmitherman 0:8a066434c28e 55 float pid_return;
srsmitherman 0:8a066434c28e 56 int pulses=0;
srsmitherman 0:8a066434c28e 57
srsmitherman 0:8a066434c28e 58 typedef struct
srsmitherman 0:8a066434c28e 59 {
srsmitherman 0:8a066434c28e 60 char command[12];
srsmitherman 0:8a066434c28e 61 void (*fnctPtr)(void);
srsmitherman 0:8a066434c28e 62 }CommandListType;
srsmitherman 0:8a066434c28e 63
srsmitherman 0:8a066434c28e 64 CommandListType CommandTable[num_commands]={
srsmitherman 0:8a066434c28e 65 {"g", &start},
srsmitherman 0:8a066434c28e 66 {"s", &stop},
srsmitherman 0:8a066434c28e 67 {"f", &forward},
srsmitherman 0:8a066434c28e 68 {"b", &reverse},
srsmitherman 0:8a066434c28e 69 {"l", &left},
srsmitherman 0:8a066434c28e 70 {"r", &right},
srsmitherman 0:8a066434c28e 71 {"u", &us_distance},
srsmitherman 0:8a066434c28e 72 {"w", &wall_follow},
srsmitherman 0:8a066434c28e 73 {"error", &bad_command}
srsmitherman 0:8a066434c28e 74 };
srsmitherman 0:8a066434c28e 75
srsmitherman 0:8a066434c28e 76 int main(void)
srsmitherman 0:8a066434c28e 77 {
srsmitherman 0:8a066434c28e 78
srsmitherman 0:8a066434c28e 79 bt.baud(115200);
srsmitherman 0:8a066434c28e 80
srsmitherman 0:8a066434c28e 81 bt.attach(&UARTIntHandler,Serial::RxIrq);
srsmitherman 0:8a066434c28e 82 NVIC_SetPriority(UART2_IRQn, 2);
srsmitherman 0:8a066434c28e 83 NVIC_SetPriority(EINT3_IRQn, 0);
srsmitherman 0:8a066434c28e 84
srsmitherman 0:8a066434c28e 85 bt.printf("Start Main\n");
srsmitherman 0:8a066434c28e 86
srsmitherman 0:8a066434c28e 87 encoder.rise(&encoderIntHandler);
srsmitherman 0:8a066434c28e 88
srsmitherman 0:8a066434c28e 89
srsmitherman 0:8a066434c28e 90 while(1)
srsmitherman 0:8a066434c28e 91 {
srsmitherman 0:8a066434c28e 92
srsmitherman 0:8a066434c28e 93 }
srsmitherman 0:8a066434c28e 94
srsmitherman 0:8a066434c28e 95 }
srsmitherman 0:8a066434c28e 96
srsmitherman 0:8a066434c28e 97 float encoderDistance(void)
srsmitherman 0:8a066434c28e 98 {
srsmitherman 0:8a066434c28e 99 return ((float)pulses*DIST_PER_PULSE);
srsmitherman 0:8a066434c28e 100 }
srsmitherman 0:8a066434c28e 101
srsmitherman 0:8a066434c28e 102 void encoderReset(void)
srsmitherman 0:8a066434c28e 103 {
srsmitherman 0:8a066434c28e 104 pulses = 0;
srsmitherman 0:8a066434c28e 105 return;
srsmitherman 0:8a066434c28e 106 }
srsmitherman 0:8a066434c28e 107
srsmitherman 0:8a066434c28e 108 void encoderIntHandler(void)
srsmitherman 0:8a066434c28e 109 {
srsmitherman 0:8a066434c28e 110 pulses++;
srsmitherman 0:8a066434c28e 111 return;
srsmitherman 0:8a066434c28e 112
srsmitherman 0:8a066434c28e 113 }
srsmitherman 0:8a066434c28e 114
srsmitherman 0:8a066434c28e 115 float getUltraSonic(void)
srsmitherman 0:8a066434c28e 116 {
srsmitherman 0:8a066434c28e 117 return 0;
srsmitherman 0:8a066434c28e 118 }
srsmitherman 0:8a066434c28e 119
srsmitherman 0:8a066434c28e 120
srsmitherman 0:8a066434c28e 121 void UARTIntHandler(void)
srsmitherman 0:8a066434c28e 122 {
srsmitherman 0:8a066434c28e 123 bt.scanf("%s",inputString);
srsmitherman 0:8a066434c28e 124 while(bt.readable()){
srsmitherman 0:8a066434c28e 125 bt.getc();
srsmitherman 0:8a066434c28e 126 }
srsmitherman 0:8a066434c28e 127
srsmitherman 0:8a066434c28e 128 bt.printf("%s\n\r", inputString);
srsmitherman 0:8a066434c28e 129
srsmitherman 0:8a066434c28e 130 commandNumber = command_int(inputString);
srsmitherman 0:8a066434c28e 131 CommandTable[commandNumber].fnctPtr();
srsmitherman 0:8a066434c28e 132 return;
srsmitherman 0:8a066434c28e 133 }
srsmitherman 0:8a066434c28e 134
srsmitherman 0:8a066434c28e 135 void start(void)
srsmitherman 0:8a066434c28e 136 {
srsmitherman 0:8a066434c28e 137 bt.printf("Move a foot.\n\r");
srsmitherman 0:8a066434c28e 138 encoderReset();
srsmitherman 0:8a066434c28e 139 forward();
srsmitherman 0:8a066434c28e 140 while(encoderDistance()<12.0){
srsmitherman 0:8a066434c28e 141 //bt.printf("%f\n\r",encoderDistance());
srsmitherman 0:8a066434c28e 142 }
srsmitherman 0:8a066434c28e 143 //bt.printf("%d\n\r",pulses);
srsmitherman 0:8a066434c28e 144 stop();
srsmitherman 0:8a066434c28e 145 return;
srsmitherman 0:8a066434c28e 146 }
srsmitherman 0:8a066434c28e 147
srsmitherman 0:8a066434c28e 148 void stop(void)
srsmitherman 0:8a066434c28e 149 {
srsmitherman 0:8a066434c28e 150 bt.printf("Stop\n");
srsmitherman 0:8a066434c28e 151 leftMotors.speed(0);
srsmitherman 0:8a066434c28e 152 rightMotors.speed(0);
srsmitherman 0:8a066434c28e 153 bt.printf("%d\n\r",pulses);
srsmitherman 0:8a066434c28e 154 return;
srsmitherman 0:8a066434c28e 155 }
srsmitherman 0:8a066434c28e 156
srsmitherman 0:8a066434c28e 157 void forward(void)
srsmitherman 0:8a066434c28e 158 {
srsmitherman 0:8a066434c28e 159 //bt.printf("Move Forward\n");
srsmitherman 0:8a066434c28e 160 leftMotors.speed(0.25);
srsmitherman 0:8a066434c28e 161 rightMotors.speed(0.25);
srsmitherman 0:8a066434c28e 162 }
srsmitherman 0:8a066434c28e 163
srsmitherman 0:8a066434c28e 164 void reverse(void)
srsmitherman 0:8a066434c28e 165 {
srsmitherman 0:8a066434c28e 166 bt.printf("Move Backward\n");
srsmitherman 0:8a066434c28e 167 leftMotors.speed(-0.25);
srsmitherman 0:8a066434c28e 168 rightMotors.speed(-0.25);
srsmitherman 0:8a066434c28e 169 }
srsmitherman 0:8a066434c28e 170
srsmitherman 0:8a066434c28e 171 void left(void)
srsmitherman 0:8a066434c28e 172 {
srsmitherman 0:8a066434c28e 173 bt.printf("Move Left\n");
srsmitherman 0:8a066434c28e 174 leftMotors.speed(0.5);
srsmitherman 0:8a066434c28e 175 rightMotors.speed(-0.5);
srsmitherman 0:8a066434c28e 176 }
srsmitherman 0:8a066434c28e 177
srsmitherman 0:8a066434c28e 178 void right(void)
srsmitherman 0:8a066434c28e 179 {
srsmitherman 0:8a066434c28e 180 bt.printf("Move Right\n");
srsmitherman 0:8a066434c28e 181 leftMotors.speed(-0.7);
srsmitherman 0:8a066434c28e 182 rightMotors.speed(0.7);
srsmitherman 0:8a066434c28e 183 }
srsmitherman 0:8a066434c28e 184
srsmitherman 0:8a066434c28e 185 void us_distance(void)
srsmitherman 0:8a066434c28e 186 {
srsmitherman 0:8a066434c28e 187 bt.printf("Ultra Sonic\n\r");
srsmitherman 0:8a066434c28e 188 rangeFinder.startMeas();
srsmitherman 0:8a066434c28e 189 wait_ms(10);
srsmitherman 0:8a066434c28e 190 if ( (rangeFinder.getMeas(range) == RANGE_MEAS_VALID) && (range < 100.0) && (range > 3.0))
srsmitherman 0:8a066434c28e 191 {
srsmitherman 0:8a066434c28e 192 bt.printf("Range = %f\n", range);
srsmitherman 0:8a066434c28e 193 }
srsmitherman 0:8a066434c28e 194
srsmitherman 0:8a066434c28e 195 }
srsmitherman 0:8a066434c28e 196
srsmitherman 0:8a066434c28e 197 void wall_follow(void)
srsmitherman 0:8a066434c28e 198 {
srsmitherman 0:8a066434c28e 199 while(1){
srsmitherman 0:8a066434c28e 200
srsmitherman 0:8a066434c28e 201 pid1.setInputLimits(4.0, 20.0);
srsmitherman 0:8a066434c28e 202 pid1.setOutputLimits( -0.6, 0.6);
srsmitherman 0:8a066434c28e 203 pid1.setSetPoint(20.0);
srsmitherman 0:8a066434c28e 204
srsmitherman 0:8a066434c28e 205 rangeFinder.startMeas();
srsmitherman 0:8a066434c28e 206 wait_ms(100);
srsmitherman 0:8a066434c28e 207 if ( (rangeFinder.getMeas(range) == RANGE_MEAS_VALID) && (range < 100.0) && (range > 3.0))
srsmitherman 0:8a066434c28e 208 {
srsmitherman 0:8a066434c28e 209 //bt.printf("Range = %f\n", range);
srsmitherman 0:8a066434c28e 210 }
srsmitherman 0:8a066434c28e 211 pid1.setProcessValue(range);
srsmitherman 0:8a066434c28e 212 pid_return = pid1.compute();
srsmitherman 0:8a066434c28e 213 bt.printf("Range: %f\n PID: %f", range, pid_return);
srsmitherman 0:8a066434c28e 214
srsmitherman 0:8a066434c28e 215 if(pid_return > 0){
srsmitherman 0:8a066434c28e 216 leftMotors.speed(MAX_SPEED - pid_return);
srsmitherman 0:8a066434c28e 217 rightMotors.speed(MAX_SPEED);
srsmitherman 0:8a066434c28e 218 }else if(pid_return < 0){
srsmitherman 0:8a066434c28e 219 leftMotors.speed(MAX_SPEED);
srsmitherman 0:8a066434c28e 220 rightMotors.speed(MAX_SPEED + pid_return);
srsmitherman 0:8a066434c28e 221 }else{
srsmitherman 0:8a066434c28e 222 leftMotors.speed(MAX_SPEED);
srsmitherman 0:8a066434c28e 223 rightMotors.speed(MAX_SPEED);
srsmitherman 0:8a066434c28e 224 }
srsmitherman 0:8a066434c28e 225 }
srsmitherman 0:8a066434c28e 226 }
srsmitherman 0:8a066434c28e 227
srsmitherman 0:8a066434c28e 228
srsmitherman 0:8a066434c28e 229 void bad_command(void)
srsmitherman 0:8a066434c28e 230 {
srsmitherman 0:8a066434c28e 231
srsmitherman 0:8a066434c28e 232
srsmitherman 0:8a066434c28e 233 }
srsmitherman 0:8a066434c28e 234
srsmitherman 0:8a066434c28e 235 int command_int(char *command)
srsmitherman 0:8a066434c28e 236 {
srsmitherman 0:8a066434c28e 237 int i = 0;
srsmitherman 0:8a066434c28e 238
srsmitherman 0:8a066434c28e 239 for(i=0;i<num_commands-1;i++)
srsmitherman 0:8a066434c28e 240 {
srsmitherman 0:8a066434c28e 241 if(strncmp(command,CommandTable[i].command,strlen(CommandTable[i].command))==0)
srsmitherman 0:8a066434c28e 242 {
srsmitherman 0:8a066434c28e 243 return i;
srsmitherman 0:8a066434c28e 244 }
srsmitherman 0:8a066434c28e 245 }
srsmitherman 0:8a066434c28e 246 return num_commands-1;
srsmitherman 0:8a066434c28e 247 }