// Each different input train configuration to Separate is handled similarly
void SeparateTestHelper(TrainCar* &train1, const std::string &which_test) {
// UNCOMMENT THIS FUNCTION WHEN YOU'RE READY TO TEST SEPARATE
std::cout << "============================================================================" << std::endl;
std::cout << "SEPARATE TRAINS " << which_test << std::endl;
SanityCheck(train1);
// record the original IDs for later comparison and statistics calculation
std::vector<int> original = RecordIDs(train1);
PrintTrain(train1);
float speed_original = CalculateSpeed(train1);
TrainCar* train2;
TrainCar* train3;
Separate(train1, train2, train3);
assert (train1 == NULL);
SanityCheck(train2);
SanityCheck(train3);
// record the IDs after separation
std::vector<int> left = RecordIDs(train2);
std::vector<int> right = RecordIDs(train3);
// calculate the number of links, unlinks, and train shifts
// (all of these counts should be kept small to minimize train yard costs
int num_unlinks, num_links, num_shifts;
SeparateStatistics(original, left, right, num_unlinks, num_links, num_shifts);
std::cout << "Separate Statistics: num unlinks = " << num_unlinks;
std::cout << ", num links = " << num_links;
std::cout << ", num shifts = " << num_shifts;
std::cout << " Total Cost: " << num_unlinks+num_links+num_shifts << std::endl;
float speed_left = CalculateSpeed(train2);
float speed_right = CalculateSpeed(train3);
float left_percent = 100.0 * (speed_original-speed_left) / speed_original;
float right_percent = 100.0 * (speed_original-speed_right) / speed_original;
if (speed_left < 0.99*speed_original) {
assert (speed_right > speed_original);
std::cout << "left train is " << std::setprecision(1) << std::fixed << left_percent
<< "% slower than the original and the right train is " << std::setprecision(1) << std::fixed
<< -right_percent << "% faster than the original." << std::endl;
} else if (speed_right < 0.99*speed_original) {
assert (speed_left > speed_original);
std::cout << "right train is " << std::setprecision(1) << std::fixed << right_percent
<< "% slower than the original and the left train is " << std::setprecision(1) << std::fixed
<< -left_percent << "% faster than the original." << std::endl;
} else {
std::cout << " left and right train speeds are equal to the original." << std::endl;
}
PrintTrain(train2);
PrintTrain(train3);
// cleanup memory usage
DeleteAllCars(train2);
DeleteAllCars(train3);
}
// This function takes a random number generator to create variety in
// the freight car weights
void ShipFreightHelper(MTRand_int32 &mtrand, int num_engines, int num_cars, int min_speed, int max_cars_per_train) {
// UNCOMMENT THIS FUNCTION WHEN YOU'RE READY TO TEST SHIP FREIGHT
// create a chain with specified # of engines engines
TrainCar* all_engines = NULL;
for (int i = 0; i < num_engines; i++) {
PushBack(all_engines, TrainCar::MakeEngine());
}
// create a chain with specified # of freight cars
TrainCar* all_freight = NULL;
for (int i = 0; i < num_cars; i++) {
// the weight for each car is randomly generated in the range of 30->100 tons
int weight = 30 + (mtrand()%15)*5;
PushBack(all_freight, TrainCar::MakeFreightCar(weight));
}
// rearrange the two structures into a collection of trains
// with the specified minimum speed & specified maximum length 12 cars
std::vector<TrainCar*> trains = ShipFreight(all_engines, all_freight, min_speed, max_cars_per_train);
// when finished, we have either used up all of the engines, or
// shipped all the freight (or both!)
assert (all_engines == NULL || all_freight == NULL);
// print the remaining engines or freight cars
if (all_engines != NULL) {
std::cout << "Remaining Unused Engines:" << std::endl;
SanityCheck(all_engines);
PrintTrain(all_engines);
}
if (all_freight != NULL) {
std::cout << "Remaining UnShipped Freight:" << std::endl;
SanityCheck(all_freight);
PrintTrain(all_freight);
}
// print the trains
std::cout << "Prepared Trains for Shipment:" << std::endl;
for (unsigned int i = 0; i < trains.size(); i++) {
SanityCheck(trains[i]);
PrintTrain(trains[i]);
// check that the speed and length rules are followed
int total_weight,num_engines,num_freight_cars,num_passenger_cars,num_dining_cars,num_sleeping_cars;
TotalWeightAndCountCars(trains[i],total_weight,num_engines,num_freight_cars,num_passenger_cars,num_dining_cars,num_sleeping_cars);
int total_cars = num_engines+num_freight_cars+num_passenger_cars+num_dining_cars+num_sleeping_cars;
float speed = CalculateSpeed(trains[i]);
assert (total_cars <= max_cars_per_train);
assert (speed >= min_speed);
}
// fully delete all TrainCar nodes to prevent memory leaks
DeleteAllCars(all_engines);
DeleteAllCars(all_freight);
for (unsigned int i = 0; i < trains.size(); i++) {
DeleteAllCars(trains[i]);
}
}
void PrintTrain(TrainCar* train) {
if (train == NULL) {
std::cout << "PrintTrain: empty train!" << std::endl;
return;
}
// Print each of the 5 rows of the TrainCar ASCII art
PrintHelper(train, 0);
PrintHelper(train, 1);
PrintHelper(train, 2);
PrintHelper(train, 3);
PrintHelper(train, 4);
// UNCOMMENT THESE ADDITIONAL STATISTICS AS YOU WORK
int total_weight = 0;
int num_engines = 0;
int num_freight_cars = 0;
int num_passenger_cars = 0;
int num_dining_cars = 0;
int num_sleeping_cars = 0;
CountEnginesAndTotalWeight(train, total_weight,num_engines,num_freight_cars, num_passenger_cars, num_dining_cars, num_sleeping_cars);
int total_cars = num_engines+num_freight_cars+num_passenger_cars+num_dining_cars+num_sleeping_cars;
float speed = CalculateSpeed(train);
std::cout << "#cars = " << total_cars;
std::cout << ", total weight = " << total_weight;
std::cout << ", speed on 2% incline = " << std::setprecision(1) << std::fixed << speed;
// If there is at least one passenger car, print the average
// distance to dining car statistic
if (num_passenger_cars > 0) {
float dist_to_dining = AverageDistanceToDiningCar(train);
if (dist_to_dining < 0) {
// If one or more passenger cars are blocked from accessing the
// dining car (by an engine car) then the distance is infinity!
std::cout << ", avg distance to dining = inf";
} else {
std::cout << ", avg distance to dining = " << std::setprecision(1) << std::fixed << dist_to_dining;
}
}
// If there is at least one sleeping car, print the closest engine
// to sleeper car statistic
if (num_sleeping_cars > 0) {
int closest_engine_to_sleeper = ClosestEngineToSleeperCar(train);
std::cout << ", closest engine to sleeper = " << closest_engine_to_sleeper;
}
std::cout << std::endl;
}
/*! \brief Speed control loop
*
* This function runs a simple P-regulator speed control loop. The duty cycle
* is only updated each time a new speed measurement is ready (on each zero-crossing).
* The time step is thus variable, so the P-gain of the P-regulator corresponds to
* a speed-varying P-gain for the continous system.
*/
static signed int SpeedControl(void)
{
unsigned long currentSpeed;
signed long speedError;
signed long dutyChange;
currentSpeed = CalculateSpeed();
speedError = (speedSetpoint - currentSpeed);
dutyChange = speedError * P_REG_K_P / P_REG_SCALING;
return dutyChange;
}
void SpeedController::Execute()
{
Serial.println("-------------------------------------------");
CalculateSpeed();
Serial.print("ReqLeft: ");
Serial.println(_ReqLeftSpeed);
Serial.print("ReqRight: ");
Serial.println(_ReqRightSpeed);
// int lf_error = K_PRO * (_ReqLeftSpeed - _LeftSpeed);
// _leftacc += ((lf_error / K_PRO) + ((lf_error - _leftlast) / K_DRV));
// _leftlast = lf_error;
// _leftacc += lf_error;
//
// if (_leftacc > (MAX_TICK_RATE)) _leftacc = MAX_TICK_RATE;
// else if (_leftacc < (-MAX_TICK_RATE * 256)) _leftacc = -MAX_TICK_RATE * 256;
//
// Serial.print("LPID: ");
// Serial.print(lf_error);
// Serial.print(" ");
// Serial.println(_leftacc);
//
// pwmL(_leftacc);
// PID
int lf_error, rt_error;
lf_error = (_ReqLeftSpeed - _LeftSpeed) * 256;
_leftacc += ((lf_error / K_PRO) + ((lf_error - _leftlast) / K_DRV));
_leftlast = lf_error;
if (_leftacc > (MAX_TICK_RATE * 256)) _leftacc = MAX_TICK_RATE * 256;
else if (_leftacc < (-MAX_TICK_RATE * 256)) _leftacc = -MAX_TICK_RATE * 256;
Serial.print("LPID: ");
Serial.print(lf_error);
Serial.print(" ");
Serial.print(_leftacc);
Serial.print(" ");
Serial.println(_leftlast);
rt_error = (_ReqRightSpeed - _RightSpeed) * 256;
_rightacc += ((rt_error / K_PRO) + ((rt_error - _rightlast) / K_DRV));
_rightlast = rt_error;
if (_rightacc > (MAX_TICK_RATE * 256)) _rightacc = MAX_TICK_RATE * 256;
else if (_rightacc < (-MAX_TICK_RATE * 256)) _rightacc = -MAX_TICK_RATE * 256;
pwmL(_leftacc/256);
pwmR(_rightacc/256);
}
void CompassController::Move()
{
double speed, wheel_Direction;
// normal compass drive stuff here
speed = CalculateSpeed();
wheel_Direction = CalculateWheelDirection();
// adjust the front of the robot to where the nose should be
double y2 = m_controller.GetRawAxis(4) * -1, x2 = m_controller.GetRawAxis(3);
double rotation = 0;
// if we're asked to point in a particular direction, do it
if (fabs(__hypot(x2, y2)) > 0.25)
{
m_noseDirection = (atan2(y2, x2) * (180/M_PI) - 90.0 );
}
else if (m_spinEvent.DoEvent())
{
// use the buttons to spin around
// idea: it may be a useful idea to eventually drift back
// to the 'real' nose position if the person lets go of the
// buttons, so the bot doesn't just keep turning unexpectedly
double big = m_bigButtonAngle;
double small = m_smallButtonAngle;
// go left
if (m_controller.GetRawButton(7))
m_noseDirection += big;
// go right
if (m_controller.GetRawButton(8))
m_noseDirection -= big;
// go left a tiny bit
if (m_controller.GetRawButton(5))
m_noseDirection += small;
// go right a tiny bit
if (m_controller.GetRawButton(6))
m_noseDirection -= small;
}
rotation = m_nosePointer.GetRotation(m_noseDirection);
m_driveController->Move(speed, wheel_Direction, rotation, m_controller.GetRawButton(1));
}
// This helper function will ship all the elements
std::vector<TrainCar*> ShipFreight(TrainCar* & all_engines,TrainCar* & all_freight, float min_speed, int cars){
std::vector<TrainCar *> a;
TrainCar* temp;
TrainCar* temp1;
TrainCar* temp2;
TrainCar* newtrain;
float speedtrain;
while(all_freight->next!=NULL)
{
temp=all_engines;
all_engines=all_engines->next;
temp->next=NULL;
temp->prev=NULL;
all_engines->prev=NULL;
speedtrain=CalculateSpeed(temp);
while(all_freight->next!=NULL && CalculateSpeed(temp)>60)
{
temp1=all_freight;
all_freight=all_freight->next;
temp1->next=NULL;
temp1->prev=NULL;
all_freight->prev=NULL;
temp2=temp;
while(temp2->next!=NULL)
{
temp2=temp2->next;
}
temp2->next=temp1;
temp1->prev=temp2;
if(CalculateSpeed(temp)<60)
{
temp2->next=NULL;
temp1->next=all_freight;
temp1->next->prev=temp1;
all_freight=all_freight->prev;
break;
}
}
//std::cout<<temp1->getID()<<"))";
a.push_back(temp);
}
// Since we need to use only 5 engines , we divide it into 2 2 1
//Engine is 1 and first we will calculate the weight according to the minimum speed given for each train engine
return a;
}
double CalculateSpeed(long long bytes, const boost::posix_time::ptime& start,
const boost::posix_time::ptime& end)
{
return CalculateSpeed(bytes, end - start);
}
/* ================= main ====================
desc: 程序入口函数
pre: 无
Post: 无
*/
void main(void) {
// Local Declarations
byte laser_history_array[LASER_HISTORY_MAX]; //激光管历史状态数组
Bool temp_usualRoad_flag = TRUE; //当前判断是否正常路段标志
Bool last_usualRoad_flag = TRUE; //上次判断是否正常路段标志
Bool stopCar_flag = FALSE; //停车标志
Bool breakCar_flag = FALSE; //刹车标志
byte startlineFlag_count = 0; //经过起始线的次数
byte laser_hitNum = 1; //照到黑线的激光管个数
byte inside_count = INSIDE_COUNT_MAX; //激光管连续在黑线范围内的次数
byte outside_count = 0; //激光管连续在黑线外的次数
byte last_error = 0; //直道加速匀速控制的上次误差
LASER_STATUS last_laserStatus = MID7; //上次激光管状态
LASER_STATUS temp_laserStatus = MID7; //当前激光管状态
int last_turnAngle = PWM1_MID; //上次舵机调整的角度
int temp_turnAngle = PWM1_MID; //当前舵机需要调整的角度
int last_speed = PWM3_FAST0; //上次速度
int temp_speed = PWM3_FAST0; //当前速度
int i;
int testcount=0; //发送激光管信息计数值定义
for(i=0;i<LASER_HISTORY_MAX;i++) {
laser_history_array[i] = MID7;
}
// Statements
DisableInterrupts;
MCUInit();
SmartcarInit();
EnableInterrupts;
for(;;) {
if(PITINTE_PINTE0 == 0) { //若PIT0采集中断为关,即道路信息采集完成
laser_hitNum = 15 - CalculateLaserHitNum(g_temp_laser_array);
temp_usualRoad_flag = IsUsualRoad (laser_hitNum); //判断是否为正常道路
if (temp_usualRoad_flag) {
temp_laserStatus = GetLaserStatus(last_laserStatus,g_temp_laser_array,laser_hitNum,&inside_count,&breakCar_flag,laser_history_array,&outside_count); //得到当前激光管状态
temp_turnAngle = CalculateAngle(temp_laserStatus); //得到舵机需要调整的转角
temp_speed = CalculateSpeed (temp_turnAngle,stopCar_flag,inside_count,outside_count); //得到需要输出的速度
}
else {
if((last_usualRoad_flag == TRUE)&&(laser_hitNum>=8&&laser_hitNum<=11)) { //一定执行
startlineFlag_count = CountStartlineFlag(startlineFlag_count,g_temp_laser_array); //计算小车经过起始线的次数
if(startlineFlag_count == 2)
stopCar_flag = TRUE; //若是第二次经过起始线，停车标志置位，即停车
StopCar(stopCar_flag);
}
} /**/
testcount++;
if(testcount%50==0){
testcount=1;
SendSmartcarInfo(g_temp_laser_array,temp_laserStatus,last_laserStatus,g_temp_pulse);//发送激光管信息
} /* */
PITINTE_PINTE0 = 1; //开PIT0采集中断
}
DerectionCtrl(temp_turnAngle); //调整舵机
if(breakCar_flag == TRUE) { //若直道入弯，反转减速刹车
BreakCar(g_temp_pulse, &breakCar_flag);
}
else
SpeedCtrl(temp_speed,g_temp_pulse,&last_error); //调整正转速度
last_speed = temp_speed; //保存当前速度
last_laserStatus = temp_laserStatus; //保存当前激光管状态
last_turnAngle = temp_turnAngle; //保存当前舵机转角
last_usualRoad_flag = temp_usualRoad_flag; //保存当前是否正常道路的标志
for(i=LASER_HISTORY_MAX-1;i>0;i--){ //保存激光管历史状态
laser_history_array[i] = laser_history_array[i-1];
}
laser_history_array[0] = temp_laserStatus;
}
} //main