Judah Okeleye / Mbed 2 deprecated SLVM

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:   mbed-rtos mbed

main.cpp@0:354a8831107d, 2014-12-03 (annotated)

Committer:
jerziboi732
Date:
Wed Dec 03 22:33:03 2014 +0000
Revision:
0:354a8831107d
Initial commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jerziboi732 0:354a8831107d 1 #include "mbed.h"
jerziboi732 0:354a8831107d 2 #include "rtos.h"
jerziboi732 0:354a8831107d 3 #include "Robot.h"
jerziboi732 0:354a8831107d 4
jerziboi732 0:354a8831107d 5 DigitalOut led1(LED1);
jerziboi732 0:354a8831107d 6 DigitalOut led2(LED2);
jerziboi732 0:354a8831107d 7 DigitalOut led3(LED3);
jerziboi732 0:354a8831107d 8 DigitalOut led4(LED4);
jerziboi732 0:354a8831107d 9
jerziboi732 0:354a8831107d 10 //RFID data from other MBed
jerziboi732 0:354a8831107d 11 DigitalIn rfIdBit1(p21);
jerziboi732 0:354a8831107d 12 DigitalIn rfIdBit0(p22);
jerziboi732 0:354a8831107d 13 InterruptIn rfIdInterrupt(p23);
jerziboi732 0:354a8831107d 14
jerziboi732 0:354a8831107d 15 //Robot code
jerziboi732 0:354a8831107d 16 Serial device(p9, p10);
jerziboi732 0:354a8831107d 17 Serial rob(p13, p14);
jerziboi732 0:354a8831107d 18 Robot TheRobot(&rob, &led1);
jerziboi732 0:354a8831107d 19
jerziboi732 0:354a8831107d 20
jerziboi732 0:354a8831107d 21
jerziboi732 0:354a8831107d 22 //Debug
jerziboi732 0:354a8831107d 23 Serial pc(USBTX, USBRX); // tx, rx
jerziboi732 0:354a8831107d 24 Mutex pc_mutex;
jerziboi732 0:354a8831107d 25
jerziboi732 0:354a8831107d 26 //Global Variables
jerziboi732 0:354a8831107d 27 float biasRate = 0.1;
jerziboi732 0:354a8831107d 28 float updateRate = 2.0;
jerziboi732 0:354a8831107d 29 float speed_modifier=1;
jerziboi732 0:354a8831107d 30 float current_speed=0;
jerziboi732 0:354a8831107d 31 int p1xdir=1,p2xdir=-1; //Direction
jerziboi732 0:354a8831107d 32 int speed_limit=0;
jerziboi732 0:354a8831107d 33
jerziboi732 0:354a8831107d 34 void limit_detect(void const *args)
jerziboi732 0:354a8831107d 35 {
jerziboi732 0:354a8831107d 36 while(1)
jerziboi732 0:354a8831107d 37 {
jerziboi732 0:354a8831107d 38 if(current_speed<speed_limit) speed_modifier=1.0;
jerziboi732 0:354a8831107d 39 else speed_modifier = speed_limit / current_speed;
jerziboi732 0:354a8831107d 40 Thread::wait(20);
jerziboi732 0:354a8831107d 41 }
jerziboi732 0:354a8831107d 42 }
jerziboi732 0:354a8831107d 43
jerziboi732 0:354a8831107d 44 void SetSpeed(void const *args)
jerziboi732 0:354a8831107d 45 {
jerziboi732 0:354a8831107d 46 while(1)
jerziboi732 0:354a8831107d 47 {
jerziboi732 0:354a8831107d 48 //Set bits
jerziboi732 0:354a8831107d 49 int RfidFlag = 0;
jerziboi732 0:354a8831107d 50 int V1 = rfIdBit1? 1:0;
jerziboi732 0:354a8831107d 51 int V0 = rfIdBit0? 1:0;
jerziboi732 0:354a8831107d 52
jerziboi732 0:354a8831107d 53 //Set flag
jerziboi732 0:354a8831107d 54 RfidFlag = V1 <<1;
jerziboi732 0:354a8831107d 55 RfidFlag |= V0;
jerziboi732 0:354a8831107d 56 if(RfidFlag == 0) continue;
jerziboi732 0:354a8831107d 57
jerziboi732 0:354a8831107d 58 //Debug
jerziboi732 0:354a8831107d 59 //pc_mutex.lock();
jerziboi732 0:354a8831107d 60 pc.printf("%d , %d ___ %d\n",rfIdBit1? 1:0, rfIdBit0?1:0, RfidFlag);
jerziboi732 0:354a8831107d 61 //pc_mutex.unlock();
jerziboi732 0:354a8831107d 62
jerziboi732 0:354a8831107d 63 //Set speed limit based on RfidFlag
jerziboi732 0:354a8831107d 64 switch(RfidFlag)
jerziboi732 0:354a8831107d 65 {
jerziboi732 0:354a8831107d 66 case 0:
jerziboi732 0:354a8831107d 67 speed_limit = 0;
jerziboi732 0:354a8831107d 68 break;
jerziboi732 0:354a8831107d 69 case 1:
jerziboi732 0:354a8831107d 70 speed_limit = 10;
jerziboi732 0:354a8831107d 71 p1xdir = 1;
jerziboi732 0:354a8831107d 72 p2xdir = -1;
jerziboi732 0:354a8831107d 73 break;
jerziboi732 0:354a8831107d 74 case 2:
jerziboi732 0:354a8831107d 75 speed_limit = 10;
jerziboi732 0:354a8831107d 76 p1xdir = -1;
jerziboi732 0:354a8831107d 77 p2xdir = 1;
jerziboi732 0:354a8831107d 78 break;
jerziboi732 0:354a8831107d 79 case 3:
jerziboi732 0:354a8831107d 80 speed_limit = 60;
jerziboi732 0:354a8831107d 81 p1xdir = 1;
jerziboi732 0:354a8831107d 82 p2xdir = 1;
jerziboi732 0:354a8831107d 83 break;
jerziboi732 0:354a8831107d 84 default:
jerziboi732 0:354a8831107d 85 break;
jerziboi732 0:354a8831107d 86
jerziboi732 0:354a8831107d 87 }
jerziboi732 0:354a8831107d 88 TheRobot.SetSelectMotorVelocity(p1xdir*speed_limit, p2xdir*speed_limit);
jerziboi732 0:354a8831107d 89
jerziboi732 0:354a8831107d 90 Thread::wait(500);
jerziboi732 0:354a8831107d 91 }
jerziboi732 0:354a8831107d 92 }
jerziboi732 0:354a8831107d 93
jerziboi732 0:354a8831107d 94 int main()
jerziboi732 0:354a8831107d 95 {
jerziboi732 0:354a8831107d 96 //Start thread
jerziboi732 0:354a8831107d 97 Thread limit_change(limit_detect);
jerziboi732 0:354a8831107d 98 //Thread Poll_IMU(CalcSpeed);
jerziboi732 0:354a8831107d 99 Thread Poll_RFID(SetSpeed);
jerziboi732 0:354a8831107d 100
jerziboi732 0:354a8831107d 101 int JS1x=0, JS2x=0; //Translated binary values from joystick
jerziboi732 0:354a8831107d 102 int p1x=0,p2x=0; //PWM power
jerziboi732 0:354a8831107d 103 char txchar=0x00;
jerziboi732 0:354a8831107d 104 char txflag=0x00;
jerziboi732 0:354a8831107d 105 device.baud(600);
jerziboi732 0:354a8831107d 106 //TheRobot.SetMotorVelocity(30);
jerziboi732 0:354a8831107d 107 while(1)
jerziboi732 0:354a8831107d 108 {
jerziboi732 0:354a8831107d 109 txchar=device.getc();
jerziboi732 0:354a8831107d 110 txflag=txchar & 0xC0;
jerziboi732 0:354a8831107d 111 switch(txflag)
jerziboi732 0:354a8831107d 112 {
jerziboi732 0:354a8831107d 113 case 0x00:
jerziboi732 0:354a8831107d 114 JS1x=(int)txchar & 0x3F;
jerziboi732 0:354a8831107d 115 break;
jerziboi732 0:354a8831107d 116 case 0xC0:
jerziboi732 0:354a8831107d 117 JS2x=(int)txchar & 0x3F;
jerziboi732 0:354a8831107d 118 break;
jerziboi732 0:354a8831107d 119 default:
jerziboi732 0:354a8831107d 120 break;
jerziboi732 0:354a8831107d 121 }
jerziboi732 0:354a8831107d 122
jerziboi732 0:354a8831107d 123
jerziboi732 0:354a8831107d 124
jerziboi732 0:354a8831107d 125 //Break info into direction and magnitude
jerziboi732 0:354a8831107d 126 //if(JS1x>31){p1xdir=-1; p1x=JS1x-32;}
jerziboi732 0:354a8831107d 127 //else {p1xdir=1; p1x=31-JS1x;}
jerziboi732 0:354a8831107d 128 //if(JS2x>31){p2xdir=1; p2x=JS2x-32;}
jerziboi732 0:354a8831107d 129 //else {p2xdir=-1; p2x=31-JS2x;}
jerziboi732 0:354a8831107d 130
jerziboi732 0:354a8831107d 131
jerziboi732 0:354a8831107d 132 current_speed=sqrt((float)p1x*(float)p1x+(float)p2x*(float)p2x);
jerziboi732 0:354a8831107d 133
jerziboi732 0:354a8831107d 134
jerziboi732 0:354a8831107d 135 //PMW output based on modifier
jerziboi732 0:354a8831107d 136 //pmwL = p1xdir * (speed_modifier * p1x) * 2;
jerziboi732 0:354a8831107d 137 //pmwR = p2xdir * (speed_modifier * p2x) * 2;
jerziboi732 0:354a8831107d 138
jerziboi732 0:354a8831107d 139 //TheRobot.SetSelectMotorVelocity(pmwL, pmwR);
jerziboi732 0:354a8831107d 140 //TheRobot.WaitForAck();
jerziboi732 0:354a8831107d 141 //TheRobot.SetSelectMotorVelocity(2, 60);
jerziboi732 0:354a8831107d 142 //wait(5);
jerziboi732 0:354a8831107d 143 //TheRobot.SetSelectMotorVelocity(1, 15);
jerziboi732 0:354a8831107d 144 //wait(5);
jerziboi732 0:354a8831107d 145 //TheRobot.SetSelectMotorVelocity(2, pmwR);
jerziboi732 0:354a8831107d 146
jerziboi732 0:354a8831107d 147 //DEBUG
jerziboi732 0:354a8831107d 148 //if(JS1x>16) led1=1;
jerziboi732 0:354a8831107d 149 //else led1=0;
jerziboi732 0:354a8831107d 150 /*if(JS1x>32) led2=1;
jerziboi732 0:354a8831107d 151 else led2=0;
jerziboi732 0:354a8831107d 152 if(JS2x>16) led3=1;
jerziboi732 0:354a8831107d 153 else led3=0;
jerziboi732 0:354a8831107d 154 if(JS2x>32) led4=1;
jerziboi732 0:354a8831107d 155 else led4=0;*/
jerziboi732 0:354a8831107d 156 }
jerziboi732 0:354a8831107d 157 }